def nonbatch(task, method, N, M): simulation_object = create_env(task) d = simulation_object.num_of_features w_true = 2*np.random.rand(d)-1 w_true = w_true / np.linalg.norm(w_true) print('If in automated mode: true w = {}'.format(w_true/np.linalg.norm(w_true))) lower_input_bound = [x[0] for x in simulation_object.feed_bounds] upper_input_bound = [x[1] for x in simulation_object.feed_bounds] w_sampler = Sampler(d) psi_set = [] s_set = [] for i in range(N): w_sampler.A = psi_set w_sampler.y = np.array(s_set).reshape(-1,1) w_samples = w_sampler.sample(M) mean_w_samples = np.mean(w_samples,axis=0) print('Samples so far: ' + str(i)) print('w estimate = {}'.format(mean_w_samples/np.linalg.norm(mean_w_samples))) print('Alignment = {}'.format(mean_w_samples.dot(w_true)/np.linalg.norm(mean_w_samples))) input_A, input_B = run_algo(method, simulation_object, w_samples) psi, s = get_feedback(simulation_object, input_A, input_B, w_true) psi_set.append(psi) s_set.append(s) w_sampler.A = psi_set w_sampler.y = np.array(s_set).reshape(-1,1) w_samples = w_sampler.sample(M) print('Samples so far: ' + str(N)) print('w estimate = {}'.format(mean_w_samples/np.linalg.norm(mean_w_samples))) print('Alignment = {}'.format(mean_w_samples.dot(w_true)/np.linalg.norm(mean_w_samples)))
def nonbatch(task, method, N, M): simulation_object = create_env(task) d = simulation_object.num_of_features lower_input_bound = [x[0] for x in simulation_object.feed_bounds] upper_input_bound = [x[1] for x in simulation_object.feed_bounds] w_sampler = Sampler(d) psi_set = [] s_set = [] input_A = np.random.uniform(low=2 * lower_input_bound, high=2 * upper_input_bound, size=(2 * simulation_object.feed_size)) input_B = np.random.uniform(low=2 * lower_input_bound, high=2 * upper_input_bound, size=(2 * simulation_object.feed_size)) psi, s = get_feedback(simulation_object, input_A, input_B) psi_set.append(psi) s_set.append(s) for i in range(1, N): w_sampler.A = psi_set w_sampler.y = np.array(s_set).reshape(-1, 1) w_samples = w_sampler.sample(M) mean_w_samples = np.mean(w_samples, axis=0) print('w-estimate = {}'.format(mean_w_samples / np.linalg.norm(mean_w_samples))) input_A, input_B = run_algo(method, simulation_object, w_samples) psi, s = get_feedback(simulation_object, input_A, input_B) psi_set.append(psi) s_set.append(s) w_sampler.A = psi_set w_sampler.y = np.array(s_set).reshape(-1, 1) w_samples = w_sampler.sample(M) print('w-estimate = {}'.format(mean_w_samples / np.linalg.norm(mean_w_samples)))
def batch(task, method, N, M, b): if N % b != 0: print('N must be divisible to b') exit(0) B = 20 * b simulation_object = create_env(task) d = simulation_object.num_of_features lower_input_bound = [x[0] for x in simulation_object.feed_bounds] upper_input_bound = [x[1] for x in simulation_object.feed_bounds] w_sampler = Sampler(d) psi_set = [] s_set = [] inputA_set = np.random.uniform(low=2 * lower_input_bound, high=2 * upper_input_bound, size=(b, 2 * simulation_object.feed_size)) inputB_set = np.random.uniform(low=2 * lower_input_bound, high=2 * upper_input_bound, size=(b, 2 * simulation_object.feed_size)) for j in range(b): input_A = inputA_set[j] input_B = inputB_set[j] psi, s = get_feedback(simulation_object, input_A, input_B) psi_set.append(psi) s_set.append(s) i = b while i < N: w_sampler.A = psi_set w_sampler.y = np.array(s_set).reshape(-1, 1) w_samples = w_sampler.sample(M) mean_w_samples = np.mean(w_samples, axis=0) print('w-estimate = {}'.format(mean_w_samples / np.linalg.norm(mean_w_samples))) print('Samples so far: ' + str(i)) inputA_set, inputB_set = run_algo(method, simulation_object, w_samples, b, B) for j in range(b): input_A = inputA_set[j] input_B = inputB_set[j] psi, s = get_feedback(simulation_object, input_B, input_A) psi_set.append(psi) s_set.append(s) i += b w_sampler.A = psi_set w_sampler.y = np.array(s_set).reshape(-1, 1) w_samples = w_sampler.sample(M) mean_w_samples = np.mean(w_samples, axis=0) print('w-estimate = {}'.format(mean_w_samples / np.linalg.norm(mean_w_samples)))
def plan_path(self): print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values data = np.genfromtxt('colliders.csv', delimiter=',', dtype='str', max_rows=1) _, lat0 = data[0].strip().split(' ') _, lon0 = data[1].strip().split(' ') # TODO: set home position to (lon0, lat0, 0) self.set_home_position(float(lon0), float(lat0), 0) # TODO: retrieve current global position curr_glb_pos = [self._longitude, self._latitude, self._altitude] # TODO: convert to current local position using global_to_local() self._north, self._east, self._down = global_to_local(curr_glb_pos, self.global_home) print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) sampler = Sampler(data, SAFETY_DISTANCE) self.polygons = sampler._polygons nodes = sampler.sample(3) print('nodes_len: ', len(nodes)) xx = [[p[0], p[1], TARGET_ALTITUDE, 0] for p in nodes] data = msgpack.dumps(xx) print(data) self.connection._master.write(data)
def create_graph_probabilistic(data, drone_altitude, safety_distance, k=10, sample_size=300): sampler = Sampler(data, drone_altitude, safety_distance) polygons = sampler._polygons print("Polygons", len(polygons)) nodes = sampler.sample(sample_size) print("Data", len(data)) print("Probabilistic node length", len(nodes)) g = nx.Graph() tree = KDTree(nodes) for n1 in nodes: # for each node connect try to connect to k nearest nodes idxs = tree.query([n1], k, return_distance=False)[0] for idx in idxs: n2 = nodes[idx] if n2 == n1: continue if can_connect(n1, n2, polygons): g.add_edge(n1, n2, weight=1) return g
def create_graph_with_nodes(data, lat0, lon0, alt): print('[start]create_grid_with_nodes') sampler = Sampler(data) global polygons polygons = sampler._polygons nodes = sampler.sample(300) graph = create_graph(nodes, 10) print("Number of edges", len(graph.edges)) grid = create_grid(data, sampler._zmax, 1) nmin = np.min(data[:, 0]) emin = np.min(data[:, 1]) start = (lat0 + nmin, lon0 + emin, alt) start_closest = closest_point(graph, start) k = np.random.randint(len(graph.nodes)) print(k, len(graph.nodes)) goal = list(graph.nodes)[k] print('start {0}'.format(start_closest)) print('goal {0}'.format(goal)) print('[end]create_grid_with_nodes') return grid, graph, start_closest, goal
def get_path(start=(0, 0, 6), goal=(331, 116, 19)): data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) north_offset = -316 east_offset = -445 sampler = Sampler(data) print("AAA") polygons = sampler._polygons print("BBB") nodes = sampler.sample(3000) # (339, 380) (441, 519) # start = (0, 0, 6) # goal = (331, 116, 19) nodes += [start, goal] print(nodes) g = create_graph(nodes, k=10, polygons=polygons) path, _ = a_star_graph(g, heuristic, start, goal) waypoints = [[int(p[0]), int(p[1]), int(p[2]), 0] for p in path] path_dict = {'path': waypoints} print(waypoints) with open('path.json', 'w') as json_file: json.dump(path_dict, json_file) with open('path.json', 'r') as json_file2: dta = json.load(json_file2) print(dta['path']) return waypoints
def nonbatch(task, method, N, M, checkpoints=None): if checkpoints is None: checkpoints = [] checkpointed_weights = [] simulation_object = create_env(task) d = simulation_object.num_of_features lower_input_bound = [x[0] for x in simulation_object.feed_bounds] upper_input_bound = [x[1] for x in simulation_object.feed_bounds] w_sampler = Sampler(d) psi_set = [] s_set = [] input_A = np.random.uniform(low=2 * lower_input_bound, high=2 * upper_input_bound, size=(2 * simulation_object.feed_size)) input_B = np.random.uniform(low=2 * lower_input_bound, high=2 * upper_input_bound, size=(2 * simulation_object.feed_size)) psi, s = get_feedback_auto( simulation_object, input_A, input_B) # psi is the difference, s is the 1 or -1 signal psi_set.append(psi) s_set.append(s) for i in range(1, N): w_sampler.A = psi_set w_sampler.y = np.array(s_set).reshape(-1, 1) w_samples = w_sampler.sample(M) mean_w_samples = np.mean(w_samples, axis=0) print('w-estimate = {}'.format(mean_w_samples / np.linalg.norm(mean_w_samples))) if i in checkpoints: checkpointed_weights.append(mean_w_samples / np.linalg.norm(mean_w_samples)) print("Weights saved at iteration {}".format(i)) input_A, input_B = run_algo(method, simulation_object, w_samples) psi, s = get_feedback_auto(simulation_object, input_A, input_B) psi_set.append(psi) s_set.append(s) w_sampler.A = psi_set w_sampler.y = np.array(s_set).reshape(-1, 1) w_samples = w_sampler.sample(M) checkpointed_weights.append(mean_w_samples / np.linalg.norm(mean_w_samples)) print('w-estimate = {}'.format(mean_w_samples / np.linalg.norm(mean_w_samples))) return checkpointed_weights
def batch(task, method, N, M, b): if N % b != 0: print('N must be divisible to b') exit(0) B = 20*b simulation_object = create_env(task) d = simulation_object.num_of_features w_true = 2*np.random.rand(d)-1 w_true = w_true / np.linalg.norm(w_true) print('If in automated mode: true w = {}'.format(w_true/np.linalg.norm(w_true))) lower_input_bound = [x[0] for x in simulation_object.feed_bounds] upper_input_bound = [x[1] for x in simulation_object.feed_bounds] w_sampler = Sampler(d) psi_set = [] s_set = [] i = 0 while i < N: w_sampler.A = psi_set w_sampler.y = np.array(s_set).reshape(-1,1) w_samples = w_sampler.sample(M) mean_w_samples = np.mean(w_samples,axis=0) print('Samples so far: ' + str(i)) print('w estimate = {}'.format(mean_w_samples/np.linalg.norm(mean_w_samples))) print('Alignment = {}'.format(mean_w_samples.dot(w_true)/np.linalg.norm(mean_w_samples))) inputA_set, inputB_set = run_algo(method, simulation_object, w_samples, b, B) for j in range(b): input_A = inputA_set[j] input_B = inputB_set[j] psi, s = get_feedback(simulation_object, input_B, input_A, w_true) psi_set.append(psi) s_set.append(s) i += b w_sampler.A = psi_set w_sampler.y = np.array(s_set).reshape(-1,1) w_samples = w_sampler.sample(M) mean_w_samples = np.mean(w_samples, axis=0) print('Samples so far: ' + str(N)) print('w estimate = {}'.format(mean_w_samples/np.linalg.norm(mean_w_samples))) print('Alignment = {}'.format(mean_w_samples.dot(w_true)/np.linalg.norm(mean_w_samples)))
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values data = np.genfromtxt('colliders.csv', delimiter=',', dtype='str', max_rows=1) _, lat0 = data[0].strip().split(' ') _, lon0 = data[1].strip().split(' ') # TODO: set home position to (lon0, lat0, 0) self.set_home_position(float(lon0), float(lat0), 0) # TODO: retrieve current global position curr_glb_pos = [self._longitude, self._latitude, self._altitude] # TODO: convert to current local position using global_to_local() self._north, self._east, self._down = global_to_local( curr_glb_pos, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) sampler = Sampler(data, SAFETY_DISTANCE) self.polygons = sampler._polygons nodes = sampler.sample(300) print('nodes_len: ', len(nodes)) g = self.create_graph(nodes, 10) print('graph_edgs: ', len(g.edges)) start = self.local_position goal = global_to_local([-122.396428, 37.795128, TARGET_ALTITUDE], self.global_home) start = self.find_closest_node(g.nodes, start) goal = self.find_closest_node(g.nodes, goal) path, cost = a_star_for_graph(g, heuristic, start, goal) print('a_star_path: ', path) path = self.prune_path(path) print('prune_path: ', path) if len(path) > 0: # Convert path to waypoints waypoints = [[p[0], p[1], TARGET_ALTITUDE, 0] for p in path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def sample_nodes(data, grid_start, grid_goal, debug): start_time = time.time() sampler = Sampler(data) # Extract all the polygons polygons = sampler._polygons nodes = sampler.sample(grid_start, grid_goal, NUM_SAMPLES) stop_time = time.time() if debug: print("Time taken to build Sampler is: {0:5.2f}s".format(stop_time - start_time)) return nodes, polygons
def read_ply(self, file_name): num_samples = self.num_samples // len(self.files_list) if self.file_index == len(self.files_list) - 1: num_samples = num_samples + (self.num_samples - (num_samples * len(self.files_list))) root, ext = os.path.splitext(file_name) if not os.path.isfile(root + ".npy"): ply = PlyData.read(file_name) vertex = ply['vertex'] (x, y, z) = (vertex[t] for t in ('x', 'y', 'z')) points = zip(x.ravel(), y.ravel(), z.ravel()) np.save(root + ".npy", points) else: points = np.load(root + ".npy") #load normals if os.path.isfile(root + "_normals" + ".ply"): if not os.path.isfile(root + "_normals" + ".npy"): ply1 = PlyData.read(root + "_normals" + ".ply") vertex = ply1['vertex'] (nx, ny, nz) = (vertex[t] for t in ('nx', 'ny', 'nz')) self.normals = np.asarray(zip(nx.ravel(), ny.ravel(), nz.ravel())) np.save(root + "_normals" + ".npy", self.normals) else: self.normals = np.load(root + "_normals" + ".npy") if self.add_noise: self.data = utils.add_noise_normal(points, std=self.nois_std) else: self.data = np.asarray(points) self.pc_diameter = utils.get_pc_diameter(self.data) self.l = self.relL*self.pc_diameter rot = utils.angle_axis_to_rotation(self.rotation_angle, self.rotation_axis) self.data = utils.transform_pc(self.data, rot) #plotutils.show_pc(self.data) #mlab.show() #TODO: better sampling print "sampling file: ", file_name self.samples, self.sample_indices = Sampler.sample(self.data, -1, min_num_point=-1, file_name=file_name, sampling_algorithm=self.sampling_algorithm) #self.samples, self.sample_indices = Sampler.sample(self.data, -1, num_samples, file_name=file_name, sampling_algorithm=self.sampling_algorithm) #self.samples = self.samples[0:num_samples] #self.sample_indices = self.sample_indices[0:num_samples] self.tree = spatial.KDTree(self.data) return self.data
def read_ply(self, file_name, num_samples=1000, sample_class_start=0, add_noise =False, noise_prob=0.3, noise_factor=0.02, noise_std=0.1, sampling_algorithm=SampleAlgorithm.Uniform, rotation_axis=[0, 0, 1], rotation_angle=0): root, ext = os.path.splitext(file_name) if not os.path.isfile(root + ".npy"): ply = PlyData.read(file_name) vertex = ply['vertex'] (x, y, z) = (vertex[t] for t in ('x', 'y', 'z')) points = zip(x.ravel(), y.ravel(), z.ravel()) np.save(root + ".npy", points) else: points = np.load(root + ".npy") #load normals if os.path.isfile(root + "_normals" + ".ply"): if not os.path.isfile(root + "_normals" + ".npy"): ply1 = PlyData.read(root + "_normals" + ".ply") vertex = ply1['vertex'] (nx, ny, nz) = (vertex[t] for t in ('nx', 'ny', 'nz')) self.normals = np.asarray(zip(nx.ravel(), ny.ravel(), nz.ravel())) np.save(root + "_normals" + ".npy", self.normals) else: self.normals = np.load(root + "_normals" + ".npy") if add_noise: print "adding noise to model.." mr = utils.model_resolution(np.array(points)) #mr = 0.404 print "model resolution: ", mr self.data = utils.add_noise_normal(np.array(points), mr, noise_std) else: self.data = np.asarray(points) rot = utils.angle_axis_to_rotation(rotation_angle, rotation_axis) self.data = utils.transform_pc(self.data, rot) #plotutils.show_pc(self.data) #mlab.show() #TODO: better sampling self.samples, self.sample_indices = Sampler.sample(self.data, -1, num_samples-1, file_name=file_name, pose=rot, sampling_algorithm=sampling_algorithm) self.tree = spatial.KDTree(self.data) self.sample_class_start = sample_class_start self.sample_class_current = sample_class_start self.num_samples = self.samples.shape[0] print "num samples: ", self.num_samples logging.basicConfig(filename='example.log',level=logging.DEBUG) return self.data
# ## Step 2 - Sample Points # # # You may want to limit the z-axis values. # In[70]: from sampling import Sampler # TODO: sample points randomly # then use KDTree to find nearest neighbor polygon # and test for collision num_samp = 500 sampler = Sampler(data) polygons = sampler._polygons nodes = sampler.sample(num_samp) print(len(nodes)) # ## Step 3 - Connect Nodes # # Now we have to connect the nodes. There are many ways they might be done, it's completely up to you. The only restriction being no edge connecting two nodes may pass through an obstacle. # # NOTE: You can use `LineString()` from the `shapely` library to create a line. Additionally, `shapely` geometry objects have a method `.crosses` which return `True` if the geometries cross paths, for instance your `LineString()` with an obstacle `Polygon()`! # In[71]: # TODO: connect nodes # Suggested method # 1) cast nodes into a graph called "g" using networkx
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 7 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values with open('colliders.csv', newline='') as f: reader = csv.reader(f) row1 = next(reader) # gets the first line lat0, lon0 = float(row1[0][5:]), float(row1[1][5:]) # TODO: set home position to (lat0, lon0, 0) self.set_home_position( lon0, lat0, 0) # set the current location to be the home position # TODO: retrieve current global position current_global_pos = (self._longitude, self._latitude, self._altitude) # TODO: convert to current local position using global_to_local() current_local_pos = global_to_local(current_global_pos, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Get the center of the grid north_offset = int(np.floor(np.min(data[:, 0] - data[:, 3]))) east_offset = int(np.floor(np.min(data[:, 1] - data[:, 4]))) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) sampler = Sampler(data) polygons = sampler._polygons # Example: sampling 1200 points and removing # ones conflicting with obstacles. nodes = sampler.sample(1200) print("Non collider nodes:", len(nodes)) t0 = time.time() # Uncomment line 162 to generate the graph from the sampled points, and comment out line 163. # Increase the Mavlink timer to avoid disconnecting from the simulator. #G = create_graph(nodes, 10, polygons) G = nx.read_gpickle( "graph_1200_SD_nodes.gpickle" ) # Comment out this line if generating the graph instead of using the saved graph print('graph took {0} seconds to build'.format(time.time() - t0)) print("Number of edges", len(G.edges)) # TODO: convert start position to current position rather than map center grid_start = (int(current_local_pos[0] - north_offset), int(current_local_pos[1] - east_offset), TARGET_ALTITUDE) # TODO: adapt to set goal as latitude / longitude position and convert goal_global_pos = (-122.394700, 37.789825, 13) goal_local_pos = global_to_local(goal_global_pos, self.global_home) grid_goal = (int(goal_local_pos[0] - north_offset), int(goal_local_pos[1] - east_offset), goal_global_pos[2]) print("goal_local_N:", goal_local_pos[0], "goal_local_E:", goal_local_pos[1], "goal_local_alt:", goal_local_pos[2]) #goal_ne = (455., 635., 20.) start_ne_g = closest_point(G, grid_start) goal_ne_g = closest_point(G, grid_goal) print('Local Start and Goal: ', start_ne_g, goal_ne_g) # Run A* to find a path from start to goal path, cost = a_star_graph(G, heuristic_graph, start_ne_g, goal_ne_g) print("Path Length:", len(path), "Path Cost:", cost) int_path = [[int(p[0]), int(p[1]), int(p[2])] for p in path] # TODO: prune path to minimize number of waypoints pruned_path = prune_path(int_path) print("Length Pruned Path:", len(pruned_path)) print("PRUNED PATH:", pruned_path) # Convert path to waypoints waypoints = [[p[0] + north_offset, p[1] + east_offset, p[2], 0] for p in pruned_path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values filename = 'colliders.csv' data = np.loadtxt(filename, delimiter=';,', dtype='str')[0].split(", ") lat0, lon0 = [float(d.split(" ")[1]) for d in data] # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) print("self.local_position ", self.local_position) # TODO: retrieve current global position current_glbl_pos = [self._longitude, self._latitude,self._altitude] print(current_glbl_pos) print(self.global_position) # TODO: convert to current local position using global_to_local() current_lcl_pos = global_to_local(current_glbl_pos, self.global_home) print("current_lcl_pos ", len(current_lcl_pos)) print('global home {0}, position {1}, local position {2}'.format(self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles _, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) # Define starting point on the grid (this is just grid center) # grid_start = (-north_offset, -east_offset) # TODO: convert start position to current position rather than map center grid_start = (int(current_lcl_pos[0]), int(current_lcl_pos[1])) # Set goal as some arbitrary position on the grid # grid_goal = (-north_offset + 10, -east_offset + 10) # Example: sampling 300 points and removing # ones conflicting with obstacles. # print("Start of sampling...") sampler = Sampler(data) polygons = sampler.polygons nodes = sampler.sample(300) nodes.insert(0, (grid_start[0], grid_start[1], 5.0)) # Build a graph using the sampled nodes # and connect each nodes to its 10th closest nodes print("Start graph building...") t0 = time.time() graph = create_graph(polygons, nodes, 11) print('graph took {0} seconds to build'.format(time.time() - t0)) start = list(graph.nodes)[0] len_path = 0 while len_path == 0: k = np.random.randint(len(graph.nodes)) goal = list(graph.nodes)[k] path, _ = a_star(graph, heuristic, start, goal) len_path = len(path) pruned_path = prune_path(path, polygons) waypoints = [[int(p[0]), int(p[1]), int(p[2]), 0] for p in pruned_path] self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
#Step 1 - Load Data # This is the same obstacle data from the previous lesson. filename = '../data/colliders.csv' data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=2) print(data) #Step 2 - Sample Points in free space from sampling import Sampler sampler = Sampler(data) polygons = sampler._polygons # Example: sampling 100 points and removing # ones conflicting with obstacles. nodes = sampler.sample(300) print(len(nodes)) #Step 3 - Connect Nodes import numpy.linalg as LA from sklearn.neighbors import KDTree def can_connect(n1, n2): l = LineString([n1, n2]) for p in polygons: if p.crosses(l) and p.height >= min(n1[2], n2[2]): return False return True
def create_prm(data): sampler = Sampler(data) polygons = sampler._polygons nodes = sampler.sample(300) g = create_graph(nodes, 10,polygons) return nodes,g
int(np.round(goal_local[1])) - east_offset, 0.0) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Sampling nodes...') t0 = time.time() print(data.shape) sampler = Sampler(data) polygons = sampler._polygons print(len(polygons)) print('Sampler took {0} seconds to build'.format(time.time() - t0)) # 1000 is veeery slow t0 = time.time() nodes = sampler.sample(200) print(nodes) nodes.append((grid_start[0], grid_start[1], 5.0)) nodes.append(grid_goal) print('Sampling took {0} seconds to build'.format(time.time() - t0)) print('Number of nodes: ', len(nodes)) print('Creating graph...') t0 = time.time() # k = 15 - https://arxiv.org/pdf/1105.1186.pdf graph = create_graph(nodes, 15, sampler.polygons) print('Graph took {0} seconds to build'.format(time.time() - t0)) print("Number of edges", len(graph.edges)) """ fig = plt.figure()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # Read lat0, lon0 from colliders.csv into floating point values # I just use `open()` method and `f.readline()` to read the first line, # which line it read depend on the times it calls, I just call it once so it's the first line, # then slice out the number with open('colliders.csv', 'r') as f: first_line_data = f.readline().rstrip().split(', ') lat0 = float(first_line_data[0][5:]) lon0 = float(first_line_data[1][5:]) # Set home position self.set_home_position(lon0, lat0, 0) # Retrieve current global position gp = self.global_position # (lon, lat, up) # Convert to current local position lp = global_to_local(gp, self.global_home) # (north, east, down) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map t0 = time.time() data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) print("Took {0} seconds to load csv file".format(time.time() - t0)) # Convert start position to current position start = (lp[0], lp[1], TARGET_ALTITUDE) t0 = time.time() sampler = Sampler(data, self.global_home) print( "Took {0} seconds to initialize Sampler class".format(time.time() - t0)) t0 = time.time() nodes = sampler.sample(30) # Set goal as some arbitrary position on the map goal = sampler.sample_goal(nodes, 37.793, -122.4) print('Local Start and Goal: ', start, goal) nodes.append(start) nodes.append(goal) print("Took {0} seconds to create {1} sample nodes.".format( time.time() - t0, len(nodes))) t0 = time.time() g = Graph(sampler.polygons, sampler.polygon_center_tree, sampler.max_poly_xy) g.create_graph(nodes, 10) print("Took {0} seconds to build graph".format(time.time() - t0)) print("Number of edges:", len(g.edges)) t0 = time.time() # Convert grid search to graph path, _ = a_star_graph(g.graph, heuristic, start, goal) print("Took {0} seconds to search path".format(time.time() - t0)) # Prune path to minimize number of waypoints t0 = time.time() path = prune_path(path) print("Took {0} seconds to prune path".format(time.time() - t0)) # Convert path to waypoints waypoints = [[int(p[0]), int(p[1]), TARGET_ALTITUDE, 0] for p in path] self.waypoints = waypoints # Send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
#Example from sampling import Sampler #elements is an array of pairs, the first member contains the element #and the second the probability of that element being picked up elements = [(i, 1 / 20) for i in range(10)] + [(10, 1 / 2)] s = Sampler(elements) #Testing correctness of my procedure, with the above elements we expect 10 to appear 50% of the time and #the others 5% of the time each. count = [0 for i in range(len(elements))] for i in range(10000): count[s.sample()] += 1 print(count)
def main_sample(): pc = utils.read_ply('E:/Fac/BMC Master/Thesis/Models/bunny/reconstruction/plytest/bun_zipper.ply') sample_points = Sampler.sample(pc, -1, 1000, sampling_algorithm=SampleAlgorithm.Uniform) mlab.points3d(sample_points[:, 0], sample_points[:, 1], sample_points[:, 2], color=(1, 1, 1), mode='point') mlab.show() print 'done'
def read_ply(self, file_name): num_samples = self.num_samples // len(self.files_list) if self.file_index == len(self.files_list) - 1: num_samples = num_samples + (self.num_samples - (num_samples * len(self.files_list))) root, ext = os.path.splitext(file_name) if not os.path.isfile(root + ".npy"): ply = PlyData.read(file_name) vertex = ply['vertex'] (x, y, z) = (vertex[t] for t in ('x', 'y', 'z')) points = zip(x.ravel(), y.ravel(), z.ravel()) np.save(root + ".npy", points) else: points = np.load(root + ".npy") if self.add_noise: self.data = utils.add_noise(points, prob=self.noise_prob, factor=self.noise_factor) else: self.data = np.asarray(points) #if self.data.shape[0] > 2e5: # self.data, _ = Sampler.sample(self.data, -1, 2e5, sampling_algorithm=self.sampling_algorithm) pc_diameter = utils.get_pc_diameter(self.data) self.l = self.relL*pc_diameter rot = utils.angle_axis_to_rotation(self.rotation_angle, self.rotation_axis) self.data = utils.transform_pc(self.data, rot) #plotutils.show_pc(self.data) #mlab.show() #TODO: better sampling print "sampling file: ", file_name self.samples, self.sample_indices = Sampler.sample(self.data, -1, num_samples, file_name=file_name, sampling_algorithm=self.sampling_algorithm) self.samples = self.samples[0:num_samples] self.sample_indices = self.sample_indices[0:num_samples] self.tree = spatial.KDTree(self.data) #TODO:Intergrate with num_samples for consistency if self.filter_bad_samples: temp_file_samples = 'temp/' + os.path.basename(file_name) + '_' + str(num_samples) + '_filter' + str(self.filter_threshold) + '.npy' print 'samples file: ', temp_file_samples if os.path.isfile(temp_file_samples): self.sample_indices = np.load(temp_file_samples) self.samples = self.data[self.sample_indices] else: self.samples, self.sample_indices = Sampler.sample(self.data, -1, num_samples*2, sampling_algorithm=self.sampling_algorithm) self.samples = self.samples[0:num_samples*2] self.sample_indices = self.sample_indices[0:num_samples*2] sample_indices_temp = [] for idx in self.sample_indices: if self.is_good_sample(self.data[idx], self.filter_threshold): sample_indices_temp.append(idx) if len(sample_indices_temp) >= num_samples: break assert (len(sample_indices_temp) >= num_samples) self.sample_indices = np.asarray(sample_indices_temp[0:num_samples]) self.samples = self.data[self.sample_indices] np.save(temp_file_samples, self.sample_indices) #plotutils.show_pc(self.samples) #mlab.show() logging.basicConfig(filename='example.log',level=logging.DEBUG) return self.data
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 10 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values lat0, lon0 = read_home_location() # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position current_global_position = [ self._longitude, self._latitude, self._altitude ] print("\nself.global_position: {0}".format(self.global_position)) print("current_global_position: {0}".format(current_global_position)) # TODO: convert to current local position using global_to_local() current_local_position = global_to_local(current_global_position, self.global_home) print("current_local_position: {0}".format(current_local_position)) print('\nglobal home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) ''' # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format(north_offset, east_offset)) # Define starting point on the grid (this is just grid center) #grid_start = (-north_offset, -east_offset) #hardcoded to middle of the grid, replacing it with below: # TODO: convert start position to current position rather than map center grid_start = ( int(np.ceil(current_local_position[0] - north_offset)), int(np.ceil(current_local_position[1] - east_offset)) ) print("grid_start: {0}".format(grid_start)) # Set goal as some arbitrary position on the grid #grid_goal = (-north_offset + 10, -east_offset + 10) #hardcoded as some location 10 m north and 10 m east of map center as dfault, replacing it with below: # TODO: adapt to set goal as latitude / longitude position and convert #goal_global_position = new_global_position(self.global_home, 15, 25) #adding 20 x 10 meters to the global_home position. goal_global_position = [-122.401055, 37.795461, 0] goal_local_position = global_to_local(goal_global_position, self.global_home) grid_goal = (int(goal_local_position[0] - north_offset), int(goal_local_position[1] - east_offset)) print("goal_local_position: {0}".format(goal_local_position)) print("grid_goal: {0}".format(grid_goal)) print("grid.shape: {0}".format(grid.shape)) if grid_goal[0] > grid.shape[0] or grid_goal[1] > grid.shape[1] or grid_goal[0] < 0 or grid_goal[1] < 0: print("\n * NOTE: Destination is outside the grid map. setting it back to start ") grid_goal = grid_start self.landing_transition() #if grid[grid_goal[0]][grid_goal[1]] > 0: # checking that the destination is not colliding with any obstacle # print("\n * NOTE: Destination is inside an obstacle. Setting it to start position. ") # grid_goal = grid_start # self.landing_transition() # REZA: Adding Skeleton median axis skeleton = medial_axis(invert(grid)) skel_start, skel_goal = find_start_goal(skeleton, grid_start, grid_goal) print('\nLocal Skelton Start and Goal: ', grid_start, grid_goal) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Local Grid Start and Goal: ', grid_start, grid_goal) # Choose one of the following A* #path, _ = a_star(grid, heuristic, grid_start, grid_goal) #Run A* on the grid path, _ = a_star(invert(skeleton).astype(np.int), heuristic, tuple(skel_start), tuple(skel_goal)) # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! print ("path: ", path) path = prune_path(path) print ("revised path: ", path) ''' # Create the smaple points print("\nStarting the Sampling ...") sampler = Sampler(data) polygons = sampler._polygons nodes = sampler.sample(100) print("Created probabilistic node samples: " + str(len(nodes))) print("\nCurrent nx version: {0}, it must be 2.1".format( nx.__version__)) # should be 2.1 print("\nStarting the path planning ...") t0 = time.time() graph = create_graph(nodes, polygons, 10) print('graph took {0} seconds to build'.format(time.time() - t0)) print("Number of edges", str(graph.number_of_edges())) #Setting my Goal position goal_global_position = [-122.401055, 37.795461, 0] goal_local_position = global_to_local(goal_global_position, self.global_home) graph_nodes_list = np.array(graph.nodes) nearest_node_index = find_nearest_waypoint(current_local_position, graph_nodes_list) start_node = graph_nodes_list[graph_nodes_list] nearest_node_index = find_nearest_waypoint(goal_local_position, graph_nodes_list) goal_node = graph_nodes_list[nearest_node_index] print("Start Node: " + str(start_node)) print("End Node: " + str(goal_node)) self.waypoints, _ = a_star_waypoints(graph, heuristic, start_node, goal_node) #Adding my actual start and end positions to the begining and the end of the waypoints_path self.waypoints.insert( 0, goal_local_position) #adding the start world position ###waypoints_path.appent() # Set self.waypoints #self.waypoints = waypoints_path # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE def can_connect(n1, n2, polygons): l = LineString([n1, n2]) for p in polygons: if p.crosses(l) and p.height >= min(n1[2], n2[2]): return False return True def create_graph(nodes, k, polygons): g = nx.Graph() tree = KDTree(nodes) for n1 in nodes: # for each node connect try to connect to k nearest nodes idxs = tree.query([n1], k, return_distance=False)[0] for idx in idxs: n2 = nodes[idx] if n2 == n1: continue if can_connect(n1, n2, polygons): g.add_edge(n1, n2, weight=1) return g # TODO: read lat0, lon0 from colliders into floating point values data_pos = np.loadtxt('colliders.csv', dtype='str', max_rows=1) (lat0, lon0) = [float(data_pos[1][:-1]), float(data_pos[3][:-1])] # TODO: set home position to (lon0, lat0, 0) (east_home, north_home, _, _) = utm.from_latlon(lat0, lon0) # TODO: retrieve current global position (east, north, _, _) = utm.from_latlon(self.global_position[1], self.global_position[0]) # TODO: convert to current local position using global_to_local() local_position = np.array([ north - north_home, east - east_home, -(self.global_position[2] - self.global_home[2]) ]) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Sample points sampler = Sampler(data) polygons = sampler._polygons nodes = sampler.sample(150) print(len(nodes)) # Connect nodes to graph g = create_graph(nodes, 10, polygons) #grid = create_grid(data, sampler._zmax, 1) # define start as first point from graph, goal point is randomized start = list(g.nodes)[0] k = np.random.randint(len(g.nodes)) print("Amount of found nodes:{0} {1}".format(k, len(g.nodes))) goal = list(g.nodes)[k] # Run A* to find path path, cost = a_star(g, heuristic, start, goal) path_pairs = zip(path[:-1], path[1:]) for (n1, n2) in path_pairs: print(n1, n2) # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) ''' # Define starting point on the grid (this is just grid center) grid_start = (int(local_position[0]-north_offset), int(local_position[1]-east_offset)) # TODO: convert start position to current position rather than map center # Set goal as some arbitrary position on the grid grid_goal = (int(local_position[0]-north_offset), int(local_position[1]-east_offset)) # TODO: adapt to set goal as latitude / longitude position and convert # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) print('Local Start and Goal: ', grid_start, grid_goal) path_, _ = a_star(grid, heuristic, grid_start, grid_goal) # TODO: prune path to minimize number of waypoints path = collinearity(path_) # TODO (if you're feeling ambitious): Try a different approach altogether! ''' # Convert path to waypoints waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in path] print(waypoints) print(grid_start, grid_goal) # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()
import matplotlib.pyplot as plt from shapely.geometry import Polygon, Point, LineString from queue import PriorityQueue filename = 'colliders.csv' data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=2) from sampling import Sampler import random random.seed(0) sampler = Sampler(data) polygons = sampler._polygons NUM_SAMPLES = 1000 nodes = sampler.sample(NUM_SAMPLES) print(len(nodes)) import numpy.linalg as LA from sklearn.neighbors import KDTree def can_connect(n1, n2): l = LineString([n1, n2]) for p in polygons: c = p._coords if p.crosses(l) and p.height >= min(n1[2], n2[2]): return False return True
import networkx as nx plt.rcParams['figure.figsize'] = 6, 6 filename = 'colliders.csv' data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=3) print(data) from sampling import Sampler sampler = Sampler(data) polygons = sampler._polygons # Example: sampling 100 points and removing # ones conflicting with obstacles. nodes = sampler.sample(100) print(len(nodes)) import numpy.linalg as LA from sklearn.neighbors import KDTree def can_connect(n1, n2): l = LineString([n1, n2]) for p in polygons: if p.crosses(l) and p.height >= min(n1[2], n2[2]): return False return True def create_graph(nodes, k):
# This is the same obstacle data from the previous lesson. filename = 'colliders.csv' data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=2) print(data) # In[35]: # flight_altitude = 3 safety_distance = 3 sampler = Sampler(data) polygons = sampler._polygons # Example: sampling 100 points and removing # ones conflicting with obstacles. nodes = sampler.sample(500) print(len(nodes)) # In[36]: import numpy.linalg as LA from sklearn.neighbors import KDTree def can_connect(n1, n2): l = LineString([n1, n2]) for p in polygons: if p.crosses(l) and p.height >= min(n1[2], n2[2]): return False return True
def plan_path(self): self.flight_state = States.PLANNING print("Searching for a path ...") TARGET_ALTITUDE = 5 SAFETY_DISTANCE = 5 self.target_position[2] = TARGET_ALTITUDE # TODO: read lat0, lon0 from colliders into floating point values home = pd.read_csv("colliders.csv", header=None, nrows=1, sep=' |,', engine='python') lat0 = home[0][0] lon0 = home[3][0] # TODO: set home position to (lon0, lat0, 0) self.set_home_position(lon0, lat0, 0) # TODO: retrieve current global position current_global_pos = [self._longitude, self._latitude, self._altitude] # TODO: convert to current local position using global_to_local() current_local_pos = global_to_local(current_global_pos, self.global_home) print('global home {0}, position {1}, local position {2}'.format( self.global_home, self.global_position, self.local_position)) # Read in obstacle map data = np.loadtxt('colliders.csv', delimiter=',', dtype='Float64', skiprows=2) # Define a grid for a particular altitude and safety margin around obstacles grid, north_offset, east_offset = create_grid(data, TARGET_ALTITUDE, SAFETY_DISTANCE) print("North offset = {0}, east offset = {1}".format( north_offset, east_offset)) # Define starting point on the grid (this is just grid center) #grid_start = (-north_offset, -east_offset) # TODO: convert start position to current position rather than map center grid_start = (int(current_local_pos[0]) - north_offset, int(current_local_pos[1]) - east_offset) # Set goal as some arbitrary position on the grid #grid_goal = (-north_offset + 10, -east_offset + 10) # TODO: adapt to set goal as latitude / longitude position and convert lat_min = 37.789658 lat_max = 37.797905 lon_max = -122.391999 lon_min = -122.402527 grid_goal = goal_location(lat_min, lat_max, lon_min, lon_max) while grid[grid_goal[0], grid_goal[1]] == 1: grid_goal = goal_location(lat_min, lat_max, lon_min, lon_max) # Run A* to find a path from start to goal # TODO: add diagonal motions with a cost of sqrt(2) to your A* implementation # or move to a different search space such as a graph (not done here) # Probabilistic roap map algorithm# sampler = Sampler(data) polygons = sampler._polygons nodes = sampler.sample(300) nodes.append(grid_start) nodes.append(grid_goal) print('Local Start and Goal: ', grid_start, grid_goal) #Select one a_star algorithm depending on the type of the search algorithm path, _ = a_star(grid, heuristic, grid_start, grid_goal) path, _ = a_star_graph(grid, heuristic_graph, grid_start, grid_goal) # TODO: prune path to minimize number of waypoints # TODO (if you're feeling ambitious): Try a different approach altogether! optimal_path = prune_path(path) # Convert path to waypoints waypoints = [[ p[0] + north_offset, p[1] + east_offset, TARGET_ALTITUDE, 0 ] for p in optimal_path] # Set self.waypoints self.waypoints = waypoints # TODO: send waypoints to sim (this is just for visualization of waypoints) self.send_waypoints()