def create_environs(obstacles, boundaries, boats, habitats): """ Given the obstacles, boundaries, boats, and habitats in a system of longtitudes and latitudes this function converts them into positions of Cartesian coordinates Parameter: obstacles: a list of Motion_plan_state objects boundaries: a list of Motion_plan_state objects boats: a list of Motion_plan_state objects habitats: a list of Motion_plan_state objects """ obstacle_list = [] boundary_list = [] boat_list = [] habitat_list = [] for obs in obstacles: pos = create_cartesian((obs.x, obs.y), ORIGIN_BOUND) obstacle_list.append(Motion_plan_state(pos[0], pos[1], size=obs.size)) for b in boundaries: pos = create_cartesian((b.x, b.y), ORIGIN_BOUND) boundary_list.append(Motion_plan_state(pos[0], pos[1])) for boat in boats: pos = create_cartesian((boat.x, boat.y), ORIGIN_BOUND) boat_list.append(Motion_plan_state(pos[0], pos[1], size=boat.size)) for habi in habitats: pos = create_cartesian((habi.x, habi.y), ORIGIN_BOUND) habitat_list.append(Motion_plan_state(pos[0], pos[1], size=habi.size)) return ([obstacle_list, boundary_list, boat_list, habitat_list])
def generate_rand_init_position(distance, quadrant): if quadrant == "1": min_auv_x = 0.0 max_auv_x = distance min_auv_y = 0.0 max_auv_y = distance min_goal_x = 0.0 max_goal_x = distance * 2.0 min_goal_y = 0.0 max_goal_y = distance * 2.0 elif quadrant == "2": min_auv_x = -distance max_auv_x = 0.0 min_auv_y = 0.0 max_auv_y = distance min_goal_x = -distance * 2.0 max_goal_x = 0.0 min_goal_y = 0.0 max_goal_y = distance * 2.0 elif quadrant == "3": min_auv_x = -distance max_auv_x = 0.0 min_auv_y = -distance max_auv_y = 0.0 min_goal_x = -distance * 2.0 max_goal_x = 0.0 min_goal_y = -distance * 2.0 max_goal_y = 0.0 else: min_auv_x = 0.0 max_auv_x = distance min_auv_y = -distance max_auv_y = 0.0 min_goal_x = 0.0 max_goal_x = distance * 2.0 min_goal_y = -distance * 2.0 max_goal_y = 0.0 auv_init_pos = Motion_plan_state(x=np.random.uniform(min_auv_x, max_auv_x), y=np.random.uniform(min_auv_y, max_auv_y), z=-5.0, theta=0) shark_init_pos = Motion_plan_state( x=np.random.uniform(min_goal_x, max_goal_x), y=np.random.uniform(min_goal_y, max_goal_y), z=-5.0, theta=0) return auv_init_pos, shark_init_pos
def main(): start = (0,0) goal = (100,100) boundary = [Motion_plan_state(0,0), Motion_plan_state(100,100)] obstacle_list = [] astar_solver = astar(start, goal, obstacle_list, boundary) astar_solver.perform_analysis(start, goal, boundary)
def build_environ(complexity, start): """ Generate the right number of randomized obstacles, habitats, and shark trajectories corresponding to the complexity level Parameter: complexity: an integer; represents the number of obstacles and habitats """ habitats = [] obstacles = [] shark_dict = {} test_a = [] test_b = [] GRID_RANGE = 10 DELTA_T = 2 # dt = 2s for shark_num in range(complexity): shark_dict = main_shark_traj_function(GRID_RANGE, shark_num+1, DELTA_T) i = 0 while i != complexity: obs_x = float(decimal.Decimal(random.randrange(33443758, 33445914))/1000000) obs_y = float(decimal.Decimal(random.randrange(-118488471, -118485219))/1000000) pos = catalina.create_cartesian((obs_x, obs_y), catalina.ORIGIN_BOUND) obs_size = int(decimal.Decimal(random.randrange(5, 50))/1) obs = Motion_plan_state(pos[0], pos[1], size=obs_size) # print ("obs generated: ", (obs.x, obs.y, obs.size)) if usable_obs(obs, start, obstacles): # print("usable") obstacles.append(obs) i += 1 for i in range(complexity): habitat_x = float(decimal.Decimal(random.randrange(33443758, 33445914))/1000000) habitat_y = float(decimal.Decimal(random.randrange(-118488471, -118485219))/1000000) pos = catalina.create_cartesian((habitat_x, habitat_y), catalina.ORIGIN_BOUND) habitat_size = int(decimal.Decimal(random.randrange(50, 120))/1) habi = Motion_plan_state(pos[0], pos[1], size=habitat_size) habitats.append(habi) for obs in obstacles: test_a.append((obs.x, obs.y, obs.size)) for habi in habitats: test_b.append((habi.x, habi.y, habi.size)) print("\n", "obstacles = ", test_a, "habitats = ", test_b) print ("Number of obstacles = ", len(obstacles), "Number of habitats = ", len(habitats)) print ("Shark trajectories = ", shark_dict) return {"obstacles" : obstacles, "habitats" : habitats, "sharks" : shark_dict}
def get_auv_trajectory(self, v, delta_t): """ Create an array of trajectory points representing a square path Parameters: v - linear velocity of the robot (m/s) delta_t - the time interval between each time stamp (sec) """ traj_list = [] t = 0 x = 760 y = 300 z = -10 for i in range(20): x = x + v * delta_t y = y theta = 0 t = t + delta_t traj_list.append( Motion_plan_state(x, y, z, theta, traj_time_stamp=t)) for i in range(20): x = x y = y + v * delta_t theta = math.pi / 2 t = t + delta_t traj_list.append( Motion_plan_state(x, y, z, theta, traj_time_stamp=t)) for i in range(20): x = x - v * delta_t y = y theta = math.pi t = t + delta_t traj_list.append( Motion_plan_state(x, y, z, theta, traj_time_stamp=t)) for i in range(20): x = x y = y - v * delta_t theta = -(math.pi) / 2 t = t + delta_t traj_list.append( Motion_plan_state(x, y, z, theta, traj_time_stamp=t)) return traj_list
def main(): obstacle_list = [Motion_plan_state(5,5,size=1),Motion_plan_state(3,6,size=2),Motion_plan_state(3,8,size=2),\ Motion_plan_state(3,10,size=2),Motion_plan_state(7,5,size=2),Motion_plan_state(9,5,size=2),Motion_plan_state(8,10,size=1)] boundary = [Motion_plan_state(0,0), Motion_plan_state(15,15)] testing1 = Performance(obstacle_list, boundary) length = testing1.performing()
def build_obstacles(complexity, start): """ Generate the right number of randomized obstacles corresponding to the complexity level Parameter: complexity: an integer represents the number of obstacles generated """ obstacles = [] test_b = [] i = 0 while i != complexity: obs_x = float(decimal.Decimal(random.randrange(33443758, 33445914))/1000000) obs_y = float(decimal.Decimal(random.randrange(-118488471, -118485219))/1000000) pos = catalina.create_cartesian((obs_x, obs_y), catalina.ORIGIN_BOUND) obs_size = int(decimal.Decimal(random.randrange(5, 50))/1) obs = Motion_plan_state(pos[0], pos[1], size=obs_size) # print ("obs generated: ", (obs.x, obs.y, obs.size)) if usable_obs(obs, start, obstacles): # print("usable") obstacles.append(obs) i += 1 for obs in obstacles: test_b.append((obs.x, obs.y, obs.size)) print("\n", "obstacles = ", test_b) print("obstacle account = ", len(obstacles)) return obstacles
def __init__(self, shark_id, x_pos_array, y_pos_array, x_vel_array=[], y_vel_array=[]): """ Note: x_pos_array and y_pos_array are passed in as array of strings when they get read from the csv file """ self.id = shark_id self.traj_pts_array = [] # keeps track of the index into shark's trajectory array self.index = 0 # decided to update the position arrays as we draw the shark's position # use these arrays to draw the shark's trajectory & for summary plots at the # end of the simulation # initialize the array with the first point, so we can do the orientation calculation # for the direction vector self.x_pos_array = [float(x_pos_array[0])] self.y_pos_array = [float(y_pos_array[0])] self.z_pos_array = [0] # use map to convert the array of strings to array of float self.x_vel_array = list(map(lambda x: float(x), x_vel_array)) self.y_vel_array = list(map(lambda y: float(y), y_vel_array)) for i in range(len(x_pos_array)): # the shark tracking video has frame rate 30 fps # which means that the time interval will be 1/30, or about 0.03 self.traj_pts_array.append(\ Motion_plan_state(x = float(x_pos_array[i]), y = float(y_pos_array[i]), traj_time_stamp = i * 0.03))
def create_grid_map(self, current_node, neighbor): """ Find the grid value of the current node's neighbor; the magnitude of the grid value is influenced by the angle of the current node to the nearest habitat and whether the current_node covers a habitat Parameter: current_node: a Node object neighbor: a position tuple of two elements (x_in_meters, y_in_meters) """ habitat_list = [] constant = 1 for habi in catalina.HABITATS: pos = catalina.create_cartesian((habi.x, habi.y), catalina.ORIGIN_BOUND) habitat_list.append( Motion_plan_state(pos[0], pos[1], size=habi.size)) target_habitat = habitat_list[0] min_distance = euclidean_dist((target_habitat.x, target_habitat.y), current_node.position) for habi in habitat_list: dist = euclidean_dist((habi.x, habi.y), current_node.position) if dist < min_distance: target_habitat = habi # compute Nj vector_1 = [ neighbor[0] - current_node.position[0], neighbor[1] - current_node.position[1] ] vector_2 = [ target_habitat.x - current_node.position[0], target_habitat.y - current_node.position[1] ] unit_vector_1 = vector_1 / np.linalg.norm(vector_1) unit_vector_2 = vector_2 / np.linalg.norm(vector_2) dot_product = np.dot(unit_vector_1, unit_vector_2) angle = np.arccos(dot_product) Nj = math.cos(angle) # compute Gj for item in habitat_list: dist = math.sqrt((current_node.position[0] - item.x)**2 + (current_node.position[1] - item.y)**2) if dist <= item.size: # current_node covers a habitat Gj = 1 else: Gj = 0 # compute grid value bj bj = constant * Nj + Gj return (bj)
def get_auv_state(self): """ Return a Motion_plan_state representing the orientation and the time stamp of the robot """ return Motion_plan_state(self.x, self.y, theta=self.theta, traj_time_stamp=self.curr_time)
def build_environ(complexity): """ Generate the right number of randomized obstacles and habitats corresponding to the complexity level Parameter: complexity: an integer; represents the number of obstacles and habitats """ habitats = [] obstacles = [] test_a = [] test_b = [] for i in range(complexity): obs_x = float( decimal.Decimal(random.randrange(33443758, 33445914)) / 1000000) obs_y = float( decimal.Decimal(random.randrange(-118488471, -118485219)) / 1000000) pos = catalina.create_cartesian((obs_x, obs_y), catalina.ORIGIN_BOUND) obs_size = int(decimal.Decimal(random.randrange(10, 30)) / 1) obs = Motion_plan_state(pos[0], pos[1], size=obs_size) obstacles.append(obs) habitat_x = float( decimal.Decimal(random.randrange(33443758, 33445914)) / 1000000) habitat_y = float( decimal.Decimal(random.randrange(-118488471, -118485219)) / 1000000) pos = catalina.create_cartesian((habitat_x, habitat_y), catalina.ORIGIN_BOUND) habitat_size = int(decimal.Decimal(random.randrange(50, 120)) / 1) habi = Motion_plan_state(pos[0], pos[1], size=habitat_size) habitats.append(habi) for obs in obstacles: test_a.append((obs.x, obs.y, obs.size)) for habi in habitats: test_b.append((habi.x, habi.y, habi.size)) print("\n", "obstacles = ", test_a, "habitats = ", test_b) return {"obstacles": obstacles, "habitats": habitats}
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 connect_to_goal_curve(self, mps1): a = (self.goal.y - mps1.y) / (np.cosh(self.goal.x) - np.cosh(mps1.x)) b = mps1.y - a * np.cosh(mps1.x) x = np.linspace(mps1.x, self.goal.x, 15) x = x.tolist() y = a * np.cosh(x) + b y = y.tolist() new_mps = Motion_plan_state(x[-2], y[-2]) new_mps.path.append(mps1) for i in range(1, len(x)): new_mps.path.append(Motion_plan_state(x[i], y[i])) new_mps.length += math.sqrt( (new_mps.path[i].x - new_mps.path[i - 1].x)**2 + (new_mps.path[i].y - new_mps.path[i - 1].y)**2) '''plt.plot(mps1.x, mps1.y, color) plt.plot(self.goal.x, self.goal.y, color) plt.plot([mps.x for mps in new_mps.path], [mps.y for mps in new_mps.path], color) plt.show()''' return new_mps
def get_random_mps(self, size_max=15): x_min, y_min, x_max, y_max = self.boundary_poly.bounds ran_x = random.uniform(x_min, x_max) ran_y = random.uniform(y_min, y_max) ran_theta = random.uniform(-math.pi, math.pi) ran_size = random.uniform(0, size_max) mps = Motion_plan_state(ran_x, ran_y, theta=ran_theta, size=ran_size) #ran_z = random.uniform(self.min_area.z, self.max_area.z) return mps
def __init__(self): #initialize start, goal, obstacle, boundary, habitats for path planning start = catalina.create_cartesian(catalina.START, catalina.ORIGIN_BOUND) self.start = Motion_plan_state(start[0], start[1]) goal = catalina.create_cartesian(catalina.GOAL, catalina.ORIGIN_BOUND) self.goal = Motion_plan_state(goal[0], goal[1]) self.obstacles = [] for ob in catalina.OBSTACLES: pos = catalina.create_cartesian((ob.x, ob.y), catalina.ORIGIN_BOUND) self.obstacles.append(Motion_plan_state(pos[0], pos[1], size=ob.size)) self.boundary = [] for b in catalina.BOUNDARIES: pos = catalina.create_cartesian((b.x, b.y), catalina.ORIGIN_BOUND) self.boundary.append(Motion_plan_state(pos[0], pos[1])) self.boat_list = [] for boat in catalina.BOATS: pos = catalina.create_cartesian((boat.x, boat.y), catalina.ORIGIN_BOUND) self.boat_list.append(Motion_plan_state(pos[0], pos[1], size=boat.size)) #testing data for habitats self.habitats = [] for habitat in catalina.HABITATS: pos = catalina.create_cartesian((habitat.x, habitat.y), catalina.ORIGIN_BOUND) self.habitats.append(Motion_plan_state(pos[0], pos[1], size=habitat.size)) #testing data for shark trajectories self.shark_dict = {1: [Motion_plan_state(-102 + (0.1 * i), -91 + (0.1 * i), traj_time_stamp=i) for i in range(1,501)], 2: [Motion_plan_state(-150 - (0.1 * i), 0 + (0.1 * i), traj_time_stamp=i) for i in range(1,501)]} #initialize path planning algorithm #A* algorithm self.astar = astar(start, goal, self.obstacles+self.boat_list, self.boundary) self.A_star_traj = [] #RRT algorithm self.rrt = RRT(self.goal, self.goal, self.boundary, self.obstacles+self.boat_list, self.habitats, freq=10) self.rrt_traj = [] #Reinforcement Learning algorithm #initialize live3Dgraph self.live3DGraph = Live3DGraph() #set up for sharks #self.load_real_shark(2) self.curr_time = 0 self.sonar_range = 50
def create_grid_map(self, current_node, neighbor): habitat_list = [] constant = 1 for habi in catalina.HABITATS: pos = catalina.create_cartesian((habi.x, habi.y), catalina.ORIGIN_BOUND) habitat_list.append( Motion_plan_state(pos[0], pos[1], size=habi.size)) print('habitats: ', habitat_list) target_habitat = habitat_list[0] min_distance = euclidean_dist((target_habitat.x, target_habitat.y), current_node.position) for habi in habitat_list: dist = euclidean_dist((habi.x, habi.y), current_node.position) if dist < min_distance: target_habitat = habi print('target: ', target_habitat) # compute Nj vector_1 = [ neighbor[0] - current_node.position[0], neighbor[1] - current_node.position[1] ] print('vector_1: ', vector_1) vector_2 = [ target_habitat.x - current_node.position[0], target_habitat.y - current_node.position[1] ] print('vector_2: ', vector_2) unit_vector_1 = vector_1 / np.linalg.norm(vector_1) unit_vector_2 = vector_2 / np.linalg.norm(vector_2) dot_product = np.dot(unit_vector_1, unit_vector_2) angle = np.arccos(dot_product) print('angle: ', angle) Nj = math.cos(angle) # compute Gj for item in habitat_list: dist = math.sqrt((current_node.position[0] - item.x)**2 + (current_node.position[1] - item.y)**2) if dist <= item.size: # current_node covers a habitat Gj = 1 else: Gj = 0 # compute grid value bj bj = constant * Nj + Gj return (bj)
def get_habitats(self): ''' get the location of all habitats within the boundary, represented as a list of motion_plan_states ''' habitats = [] #testing habitat lists habitats = [Motion_plan_state(750, 300, size=5), Motion_plan_state(750, 320, size=2), Motion_plan_state(780, 240, size=10),\ Motion_plan_state(775, 330, size=3), Motion_plan_state(760, 320, size=5), Motion_plan_state(770, 250, size=4),\ Motion_plan_state(800, 295, size=4), Motion_plan_state(810, 320, size=5), Motion_plan_state(815, 300, size=5),\ Motion_plan_state(825, 330, size=6), Motion_plan_state(830, 335, size=5)] return habitats
def habitats_time_spent(self, current_node): """ Find the approximate time spent in habitats if the current node is within the habitat(s) Parameter: current_node: a position tuple of two elements (x,y) habitats: a list of motion_plan_state """ dist_in_habitats = 0 velocity = 1 # velocity of exploration in m/s for habi in catalina.HABITATS: pos_habi = create_cartesian((habi.x, habi.y), catalina.ORIGIN_BOUND) dist, _ = self.get_distance_angle( Motion_plan_state(pos_habi[0], pos_habi[1], size=habi.size), Motion_plan_state(current_node.position[0], current_node.position[1])) if dist <= habi.size: dist_in_habitats += 10 return dist_in_habitats / velocity
def connect_to_goal(self, mps, exp_rate, dist_to_end=float("inf")): new_mps = Motion_plan_state(mps.x, mps.y) d, theta = self.get_distance_angle(new_mps, self.goal) new_mps.path = [new_mps] if dist_to_end > d: dist_to_end = d n_expand = math.floor(dist_to_end / exp_rate) for _ in range(n_expand): new_mps.x += exp_rate * math.cos(theta) new_mps.y += exp_rate * math.sin(theta) new_mps.path.append(Motion_plan_state(new_mps.x, new_mps.y)) d, _ = self.get_distance_angle(new_mps, self.goal) if d <= dist_to_end: new_mps.path.append(self.goal) new_mps.path[0] = mps return new_mps
def get_random_mps(self, size_max=15): x_max = max([mps.x for mps in self.boundary]) x_min = min([mps.x for mps in self.boundary]) y_max = max([mps.y for mps in self.boundary]) y_min = min([mps.y for mps in self.boundary]) ran_x = random.uniform(x_min, x_max) ran_y = random.uniform(y_min, y_max) ran_theta = random.uniform(-math.pi, math.pi) ran_size = random.uniform(0, size_max) mps = Motion_plan_state(ran_x, ran_y, theta=ran_theta, size=ran_size) #ran_z = random.uniform(self.min_area.z, self.max_area.z) return mps
def __init__(self, x, y, z, theta, velocity_1, w_1, curr_traj_pt_index, i): self.state = Motion_plan_state(x, y, z=0, theta=0) self.velocity_1 = velocity_1 self.w_1 = w_1 self.curr_traj_pt_index = 0 self.i = i self.x_list = [] self.y_list = [] self.z_list = [] self.x_list += [self.state.x] self.y_list += [self.state.y] self.z_list += [self.state.z] self.sensor_time = const.NUM_ITER_FOR_NEW_SENSOR_DATA self.shark_sensor_data_list = []
def main(): start = create_cartesian((33.444686, -118.484716), catalina.ORIGIN_BOUND) print("start: ", start) # print ('start: ', start) # print ('goal: ', goal) obstacle_list = [] boundary_list = [] habitat_list = [] goal_list = [] weights = [0, 10, 10] final_cost = [] for obs in catalina.OBSTACLES: pos = create_cartesian((obs.x, obs.y), catalina.ORIGIN_BOUND) obstacle_list.append(Motion_plan_state(pos[0], pos[1], size=obs.size)) for b in catalina.BOUNDARIES: pos = create_cartesian((b.x, b.y), catalina.ORIGIN_BOUND) boundary_list.append(Motion_plan_state(pos[0], pos[1])) for habi in catalina.HABITATS: pos = catalina.create_cartesian((habi.x, habi.y), catalina.ORIGIN_BOUND) habitat_list.append(Motion_plan_state(pos[0], pos[1], size=habi.size)) goal1 = create_cartesian((33.445779, -118.486976), catalina.ORIGIN_BOUND) print("goal: ", goal1) astar_solver = astar(start, goal1, obstacle_list, boundary_list) final_path_mps = astar_solver.astar(habitat_list, obstacle_list, boundary_list, start, goal1, weights) print("\n", "final cost: ", final_path_mps[1][0]) print("\n", "final path: ", final_path_mps[0])
def create_trajectory_list(self, traj_list): """ Run this function to update the trajectory list by including intermediate positions Parameters: traj_list - a list of Motion_plan_state, represent positions of each node """ time_stamp = 0.1 #constant_gain = 1 trajectory_list = [] step = 0 for i in range(len(traj_list) - 1): trajectory_list.append(traj_list[i]) x1 = traj_list[i].x y1 = traj_list[i].y x2 = traj_list[i + 1].x y2 = traj_list[i + 1].y dx = abs(x1 - x2) dy = abs(y1 - y2) dist = math.sqrt(dx**2 + dy**2) velocity = 1 time = dist / velocity counter = 0 while counter < time: if x1 < x2: x1 += time_stamp * velocity if y1 < y2: y1 += time_stamp * velocity step += time_stamp counter += time_stamp trajectory_list.append( Motion_plan_state(x1, y1, traj_time_stamp=step)) trajectory_list.append(traj_list[i + 1]) return trajectory_list
def generate_rand_obstacles(auv_init_pos, shark_init_pos, num_of_obstacles): """ """ obstacle_array = [] for _ in range(num_of_obstacles): obs_x = np.random.uniform(MIN_X, MAX_X) obs_y = np.random.uniform(MIN_Y, MAX_Y) obs_size = np.random.randint(1, 5) while validate_new_obstacle([obs_x, obs_y], obs_size, auv_init_pos, shark_init_pos, obstacle_array): obs_x = np.random.uniform(MIN_X, MAX_X) obs_y = np.random.uniform(MIN_Y, MAX_Y) obstacle_array.append( Motion_plan_state(x=obs_x, y=obs_y, z=-5, size=obs_size)) return obstacle_array
def collision_free(self, position, obstacleList): # obstacleList lists of motion plan state """ Check if the current position is collision-free Parameter: position - a tuple with two elements, x and y coordinates obstacleList - a list of Motion_plan_state objects """ position_mps = Motion_plan_state(position[0], position[1]) for obstacle in obstacleList: d, _ = self.get_distance_angle(obstacle, position_mps) if d <= obstacle.size: return False # collision return True
def collision_free(self, position, obstacleList): # obstacleList lists of motion plan state """ Check if the input position collide with any of the obstacles Parameters: start_pos -- a position tuple (x, y) obstacle_list -- a list of Motion_plan_state objects, obstacles Returns: True if start_pos does not collide with obstacle_list False if start_pos collides with obstacle_list """ position_mps = Motion_plan_state(position[0], position[1]) for obstacle in obstacleList: d, _ = self.get_distance_angle(obstacle, position_mps) if d <= obstacle.size: return False return True
def summary_4(rrt, habitats, shark_dict, weight): # res = rrt.exploring(Motion_plan_state(-200, 0), habitats, 0.5, 5, 2, 50, traj_time_stamp=True, max_plan_time=10, max_traj_time=500, plan_time=True, weights=weight) # traj = res["path"][0] # print(traj, res['cost']) res = rrt.replanning(Motion_plan_state(-200, 0), habitats, 10.0, 500.0, 0.1) traj = res[0] # print(traj, res[2]) #initialize total_cost = 0 # num_steps = 0 t = 1 visited = [False for _ in range(len(habitats))] #helper curr = 0 time_diff = float("inf") bin_list = list(shark_dict.keys()) curr_time = 0 while t <= (traj[-1].traj_time_stamp // 1): diff = abs(traj[curr].traj_time_stamp - t) while diff <= time_diff: time_diff = diff curr += 1 if curr == len(traj) - 1: break diff = abs(traj[curr].traj_time_stamp - t) curr = curr - 1 time_diff = float("inf") if traj[curr].traj_time_stamp > bin_list[curr_time][1]: curr_time += 1 AUVGrid = shark_dict[bin_list[curr_time]] cost, visited = habitat_shark_cost_point(traj[curr], habitats, visited, AUVGrid, weight) total_cost += cost t += 1 # num_steps += 1 return total_cost / traj[-1].traj_time_stamp
def get_auv_sensor_measurements(self, curr_time): """ Return an Motion_plan_state object that represents the measurements of the auv's x,y,z,theta position with a time stamp The measurement has random gaussian noise # 0 is the mean of the normal distribution you are choosing from # 1 is the standard deviation of the normal distribution # np.random.normal returns a single sample drawn from the parameterized normal distribution # we actually omitted the third parameter which determines the number of samples that we would like to draw """ x = self.state.x + np.random.normal(0, 1) y = self.state.y + np.random.normal(0, 1) z = self.state.z + np.random.normal(0, 1) #theta = angle_wrap(self.state.theta + np.random.normal(0,0.05)) theta = 0 return Motion_plan_state(x, y, z, theta, curr_time)
def habitats_time_spent(self, current_node): """ Get the approximate time spent in habitats if the current node is within the habitat(s) Parameters: current_node -- a position tuple (x,y) Returns: Time spent in habitats """ dist_in_habitats = 0 velocity = 1 for habi in catalina.HABITATS: pos_habi = create_cartesian((habi.x, habi.y), catalina.ORIGIN_BOUND) dist, _ = self.get_distance_angle(Motion_plan_state(pos_habi[0], pos_habi[1], size=habi.size), Motion_plan_state(current_node.position[0], current_node.position[1])) if dist <= habi.size: dist_in_habitats += 10 return dist_in_habitats/velocity
def connect_to_goal_curve_alt(self, mps, exp_rate): new_mps = Motion_plan_state(mps.x, mps.y, theta=mps.theta, traj_time_stamp=mps.traj_time_stamp) theta_0 = new_mps.theta _, theta = self.get_distance_angle(mps, self.goal) diff = theta - theta_0 diff = self.angle_wrap(diff) if abs(diff) > math.pi / 2: return #polar coordinate r_G = math.hypot(self.goal.x - new_mps.x, self.goal.y - new_mps.y) phi_G = math.atan2(self.goal.y - new_mps.y, self.goal.x - new_mps.x) #arc phi = 2 * self.angle_wrap(phi_G - new_mps.theta) radius = r_G / (2 * math.sin(phi_G - new_mps.theta)) length = radius * phi if phi > math.pi: phi -= 2 * math.pi length = -radius * phi elif phi < -math.pi: phi += 2 * math.pi length = -radius * phi new_mps.length += length ang_vel = phi / (length / exp_rate) #center of rotation x_C = new_mps.x - radius * math.sin(new_mps.theta) y_C = new_mps.y + radius * math.cos(new_mps.theta) n_expand = math.floor(length / exp_rate) for i in range(n_expand + 1): new_mps.x = x_C + radius * math.sin(ang_vel * i + theta_0) new_mps.y = y_C - radius * math.cos(ang_vel * i + theta_0) new_mps.theta = ang_vel * i + theta_0 new_mps.path.append( Motion_plan_state(new_mps.x, new_mps.y, theta=new_mps.theta, plan_time_stamp=time.time() - self.t_start)) return new_mps