def get_samples_from_new_map(): for _ in range(20): try: with ExpectTimeout(3): world = World.fixed_block(lower_bounds=(-2, -2, 0), upper_bounds=(3, 2, 2), block_width=0.5, block_height=1.5, num_blocks=4, robot_radii=0.25, margin=0.2) break except TimeoutError: pass resolution = (.25, .25, .25) margin = .2 occ_map = OccupancyMap(world, resolution, margin) start = world.world['start'] # Start point, shape=(3,) goal = world.world['goal'] # Goal point, shape=(3,) my_path = graph_search(world, resolution, margin, start, goal, False)[1:-1] start = my_path[0] goal = my_path[-1] args = get_args() args.go = goal args.st = start args.world = world args.occ_map = occ_map action_List = np.zeros((my_path.shape)) discretized_path = np.zeros((my_path.shape)) for i in range(discretized_path.shape[0]): discretized_path[i, :] = args.occ_map.metric_to_index(my_path[i, :]) for i in range(discretized_path.shape[0]): try: action_List[i, :] = discretized_path[i + 1] - discretized_path[i] except: action_List[i, :] = np.zeros(3) discretized_path = discretized_path.astype(int) action_List = action_List.astype(int) for i in range(len(action_List)): args.optimal_path_length += np.linalg.norm(action_List[i]) start_index = discretized_path[0] goal_index = discretized_path[-1] args.start = start_index args.goal = goal_index args.search_range = 20 warm_up_set, warm_up_labels = get_all_warm_up(args, discretized_path, action_List) return args, warm_up_set, warm_up_labels
from flightsim.crazyflie_params import quad_params from flightsim.simulate import Quadrotor, simulate, ExitStatus from flightsim.world import World from flightsim.world import ExpectTimeout import numpy as np quadrotor = Quadrotor(quad_params) robot_radius = 0.25 fig = plt.figure('A* Path, Waypoints, and Trajectory') ax = Axes3Ds(fig) for _ in range(20): try: with ExpectTimeout(3): world = World.fixed_block(lower_bounds=(-2,-2,0),upper_bounds=(3,2,2),block_width=0.5,block_height=1.5,num_blocks=4,robot_radii=robot_radius,margin=0.2) break except TimeoutError: pass world.draw(ax) start=world.world['start'] goal=world.world['goal'] ax.plot([start[0]], [start[1]], [start[2]], 'go', markersize=5, markeredgewidth=3, markerfacecolor='none') ax.plot([goal[0]], [goal[1]], [goal[2]], 'ro', markersize=5, markeredgewidth=3, markerfacecolor='none') plt.show()