def main(): weights = [0, 10, 10] start_cartesian = create_cartesian((33.446056, -118.489111), catalina.ORIGIN_BOUND) print(start_cartesian) start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2)) print("start: ", start) # convert to environment in casrtesian coordinates environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS) obstacle_list = environ[0] boundary_list = environ[1] boat_list = environ[2] habitat_list = environ[3] astar_solver = astar(start, obstacle_list + boat_list, boundary_list) final_path_mps = astar_solver.astar(habitat_list, obstacle_list + boat_list, boundary_list, start, 400, weights) print("\n", "final trajectory: ", final_path_mps[0]) print("\n", "cost of each node on the final trajectory: ", final_path_mps[1])
def generate_random_start_pos(self): """ Generate a random starting position within bounds and collision-free Parameters: NONE Returns: A position tuple (x, y) """ # Set up the environment environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS) obstacle_list = environ[0] + environ[2] boundary_list = environ[1] # Initialize doable to Flase doable = False while doable == False: random_start_position = (random.randint(-500, 100), random.randint(-200, 200)) # check if no collision if self.collision_free(random_start_position, obstacle_list): # check if within bounds if self.within_bounds(boundary_list, random_start_position): doable = True return random_start_position if doable == False: return (-100,0)
def sharks_vs_costTime(times, start, XLIST, weights, pathLenLimit, total_traj_time): """ Keep the information of shark trajectories and habitats constant to see the association between the number of obstacles and cost Parameter: times: an integer represents the number of iterations start: a position tuple of two elements in the system of longtitudes and latitudes XLIST: a list of integers each of which signifies the complexity level(i.e. item account) shark_dict: a list of Motion_plan_state objects; represents multiple shark trajectories weights: a list of four integers; we use [0, 10, 10, 100] total_traj_time: in seconds; set it to 200s """ COST = [] TIME = [] start_cartesian = catalina.create_cartesian(start, catalina.ORIGIN_BOUND) start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2)) print ("start: ", start) environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS) habitat_list = environ[3] boundary_list = environ[1] obstacle_list = environ[0]+environ[2] for complexity in XLIST: # for each kind of environment cost_list = [] time_list = [] for sim in range(times): # run a number of simulations shark_dict = build_sharkDict(complexity) astar_solver = astar(start, obstacle_list, boundary_list, habitat_list, {}, shark_dict, AUV_velocity=1) start_time = timeit.timeit() final_path_mps = astar_solver.astar(pathLenLimit, weights, shark_dict) end_time = timeit.timeit() peri_boundary = cal_boundary_peri(astar_solver) traj_node = final_path_mps["node"] traj_mps = final_path_mps["path"] print ("\n", "Final trajectory : ", traj_mps) cost = get_cost(traj_node, traj_mps, peri_boundary, total_traj_time, habitat_list, astar_solver.sharkGrid, weights) cost_list.append(cost) print ("\n", "cost list: ", cost_list) time_list.append(abs(end_time-start_time)) print ("\n", "time list: ", time_list) if len(cost_list) >= 1: COST.append(mean(cost_list)) print ("\n","cost values: ", COST) if len(time_list) >= 1: TIME.append(mean(time_list)) print("\n", "time values: ", TIME) return {"cost" : COST, "time" : TIME}
def main(): weights = [0, 10, 10, 100] start_cartesian = create_cartesian((33.446198, -118.486652), catalina.ORIGIN_BOUND) start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2)) print ("start: ", start) # convert to environment in casrtesian coordinates environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS) obstacle_list = environ[0] boundary_list = environ[1] boat_list = environ[2] habitat_list = environ[3] # testing data for shark trajectories shark_dict = {1: [Motion_plan_state(-120 + (0.3 * i), -60 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 2: [Motion_plan_state(-65 - (0.3 * i), -50 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 3: [Motion_plan_state(-110 + (0.3 * i), -40 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 4: [Motion_plan_state(-105 - (0.3 * i), -55 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 5: [Motion_plan_state(-120 + (0.3 * i), -50 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 6: [Motion_plan_state(-85 - (0.3 * i), -55 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 7: [Motion_plan_state(-270 + (0.3 * i), 50 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 8: [Motion_plan_state(-250 - (0.3 * i), 75 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 9: [Motion_plan_state(-260 - (0.3 * i), 75 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 10: [Motion_plan_state(-275 + (0.3 * i), 80 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)]} astar_solver = astar(start, obstacle_list+boat_list, boundary_list, habitat_list, {}, shark_dict, AUV_velocity=1) final_path_mps = astar_solver.astar(300, weights, shark_dict) print ("\n", "Final Trajectory: ", final_path_mps["path"]) print ("\n", "Trajectory Length: ", final_path_mps["path length"]) print ("\n", "Trajectory Cost: ", final_path_mps["cost"]) print ("\n", "Trajectory Cost List: ", final_path_mps["cost list"]) boundary_poly = [] for pos in boundary_list: boundary_poly.append((pos.x, pos.y)) boundary = Polygon(boundary_poly) # a Polygon object that represents the boundary of our workspace sharkOccupancyGrid = SharkOccupancyGrid(shark_dict, 10, boundary, 50, 50) grid_dict = sharkOccupancyGrid.convert() plot(sharkOccupancyGrid, grid_dict[0], final_path_mps["path"])
def main(): weights = [0, 10, 50] start_cartesian = create_cartesian((33.445089, -118.486933), catalina.ORIGIN_BOUND) start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2)) print ("start: ", start) # convert to environment in casrtesian coordinates environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS) obstacle_list = environ[0] boundary_list = environ[1]+environ[2] habitat_list = environ[3] single_AUV = singleAUV(start, obstacle_list, boundary_list, habitat_list, []) final_traj = single_AUV.astar(habitat_list, obstacle_list, boundary_list, start, 200, weights) print ("\n", "final trajectory: ", final_traj["path"]) print ("\n", "Trajectory length: ", len(final_traj["path"])) print ("\n", "cost of each node on the final trajectory: ", final_traj["cost"])
def display_single_astar_trajectory(self): """ Visualize the planned trajectory Parameter: NONE Returns: None """ start = (self.x, self.y) # Create the environment environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS) obstacle_list = environ[0] boundary_list = environ[1]+environ[2] habitat_list = environ[3] # Create a single_AUV object single_AUV = singleAUV(start, obstacle_list, boundary_list, habitat_list, [])
def multi_AUV_planner_fixed_start(self, pathLenLimit, weights, start_position_list): """ Find the optimal path for each AUV with fixed start positions Parameter: pathLenLimit -- the length limit of the planned trajectory in meters weights -- a list of three coefficients [w1, w2, w3] start_position_list -- a list of starting positions (x, y) Returns: dict -- a dictionary with "traj" matches a list of trajectory lists, "costs" matches a list of costs corresponding to each trajectory list """ # Create environment environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS) habitat_list = environ[3] # Create permutations of the starting position list start_pos_permu_list = permutations(start_position_list) for AUV_travel_order_list in start_pos_permu_list: self.multiAUV_habitat_open_list = habitat_list[:] # Reset the habitat coverage self.multiAUV_habitat_closed_list = [] # All habitats are open for start in AUV_travel_order_list: print ("\n", "STARTING AT: ", start) # Create a new object, single_AUV single_AUV = singleAUV(start, self.obstacle_list, self.boundary_list, self.multiAUV_habitat_open_list, self.multiAUV_habitat_closed_list) # Plan path for the single_AUV object single_planner = single_AUV.astar(start, pathLenLimit, weights) # Append the new path self.trajectories.append(single_planner["path"]) # Append the new cost self.costs.append(single_planner["cost"]) print("\n", "Traj: ", single_planner["path"]) # Update the overall habitat coverage self.multiAUV_habitat_open_list = single_AUV.habitat_open_list[:] self.multiAUV_habitat_closed_list = single_AUV.habitat_closed_list[:] return {"trajs" : self.trajectories, "costs" : self.costs}
def display_multi_astar_trajectory(self, numAUV): """ Visualize multiple trajectories Parameter: numAUV -- the number of AUVs Returns: NONE """ # Generate a random starting position random_start_pos = self.generate_random_start_pos() print ("start at ", random_start_pos) # Create the environment environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS) obstacle_list = environ[0] boundary_list = environ[1] + environ[2] habitat_list = environ[3] # Create a multi_AUV object AUVS = multiAUV(numAUV, random_start_pos, habitat_list, boundary_list, obstacle_list)
def iterate(times): weights = [0, 10, 10, 100] pathLenLimit = 200 # in meters total_traj_time = 200 # in seconds start_cartesian = catalina.create_cartesian((33.446198, -118.486652), catalina.ORIGIN_BOUND) start = (round(start_cartesian[0], 2), round(start_cartesian[1], 2)) print ("start: ", start) environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS) obstacle_list = environ[0] boundary_list = environ[1] boat_list = environ[2] x_list = [1, 3, 5, 7, 9] y_list = [] # holds averaged costs z_list = [] # holds averaged planning time '''shark_dict = {1: [Motion_plan_state(-120 + (0.3 * i), -60 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 2: [Motion_plan_state(-65 - (0.3 * i), -50 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 3: [Motion_plan_state(-110 + (0.3 * i), -40 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 4: [Motion_plan_state(-105 - (0.3 * i), -55 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 5: [Motion_plan_state(-120 + (0.3 * i), -50 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 6: [Motion_plan_state(-85 - (0.3 * i), -55 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 7: [Motion_plan_state(-270 + (0.3 * i), 50 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 8: [Motion_plan_state(-250 - (0.3 * i), 75 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 9: [Motion_plan_state(-260 - (0.3 * i), 75 + (0.3 * i), traj_time_stamp=i) for i in range(1,201)], 10: [Motion_plan_state(-275 + (0.3 * i), 80 - (0.3 * i), traj_time_stamp=i) for i in range(1,201)]} ''' for complexity in x_list: # for each kind of environment cost_list = [] time_list = [] for sim in range(times): # run a number of simulations environ_dict = build_environ(complexity, start) # returns {"obstacles" : obstacles, "habitats" : habitats} final_obstacle_list = obstacle_list+boat_list+environ_dict["obstacles"] final_habitat_list = environ_dict["habitats"] shark_dict = environ_dict["sharks"] astar_solver = astar(start, final_obstacle_list, boundary_list, final_habitat_list, {}, shark_dict, AUV_velocity=1) start_time = timeit.timeit() final_path_mps = astar_solver.astar(pathLenLimit, weights, shark_dict) end_time = timeit.timeit() peri_boundary = cal_boundary_peri(astar_solver) traj_node = final_path_mps["node"] traj_mps = final_path_mps["path"] print ("\n", "Final trajectory : ", traj_mps) cost = get_cost(traj_node, traj_mps, peri_boundary, total_traj_time, final_habitat_list, astar_solver.sharkGrid, weights) cost_list.append(cost) print ("\n", "cost list: ", cost_list) time_list.append(abs(end_time-start_time)) print ("\n", "time list: ", time_list) if len(cost_list) >= 1: y_list.append(mean(cost_list)) print ("\n","y values: ", y_list) if len(time_list) >= 1: z_list.append(mean(time_list)) print("\n", "z values: ", z_list) return {"Y" : y_list, "Z" : z_list}
end = time.time() time_list.append((end - start)) res.append(statistics.mean(time_list)) return res #initialize start, goal, obstacle, boundary, habitats for path planning # start = catalina.create_cartesian(catalina.START, catalina.ORIGIN_BOUND) # start = Motion_plan_state(start[0], start[1]) # goal = catalina.create_cartesian(catalina.GOAL, catalina.ORIGIN_BOUND) # goal = Motion_plan_state(goal[0], goal[1]) # convert to environment in casrtesian coordinates environ = catalina.create_environs(catalina.OBSTACLES, catalina.BOUNDARIES, catalina.BOATS, catalina.HABITATS) obstacles = environ[0] + environ[2] boundary = environ[1] habitats = environ[3] boundary_poly = [(mps.x, mps.y) for mps in boundary] boundary_poly = Polygon(boundary_poly) cell_list = splitCell(boundary_poly, 10) # testing data for shark trajectories shark_dict1 = { 1: [ Motion_plan_state(-120 + (0.2 * i), -60 + (0.2 * i), traj_time_stamp=i) for i in range(1, 501) ],