def __call__(self): """ Randomly sample point in area while sampling goal point at a specified rate. Return: gennav.utils.RobotState: The sampled robot state. """ if random.random() > self.goal_sample_p: new_point = Point() new_point.x = random.uniform(self.min_x, self.max_x) new_point.y = random.uniform(self.min_y, self.max_y) return RobotState(position=new_point) else: return self.goal
def uniform_random_sampler(sample_area): """ Randomly sample point in sample area Args: sample_area(tuple): area to sample point in (min and max) Return: Randomly selected point as a Point(gennav.utils.geometry.Point). """ new_point = Point() new_point.x = random.uniform(sample_area[0], sample_area[1]) new_point.y = random.uniform(sample_area[0], sample_area[1]) return new_point
def __call__(self): """Randomly sample point in area while sampling goal point at a specified rate. Return: gennav.utils.RobotState: The sampled robot state. """ if random.random() > self.goal_sample_p: theta = 2 * pi * random.random() u = random.random() * self.r new_point = Point() new_point.x = self.centre.x + u * cos(theta) new_point.y = self.centre.y + u * sin(theta) return RobotState(position=new_point) else: return self.goal
def uniform_random_circular_sampler(r): """Randomly samples point in a circular region of radius r around the origin. Args: r: radius of the circle. Return: Randomly selected point as Point(gennav.utils.geometry.Point). """ # Generate a random angle theta. theta = 2 * pi * random.random() u = random.random() + random.random() if u > 1: a = 2 - u else: a = u new_point = Point() new_point.x = r * a * cos(theta) new_point.y = r * a * sin(theta) return new_point
def uniform_adjustable_random_sampler(sample_area, goal, goal_sample_rate): """ Randomly sample point in area while sampling goal point at a specified rate. Args: sample_area(tuple): area to sample point in (min and max) goal(gennav.utils.geometry.Point):Point representing goal point. goal_sample_rate: number between 0 and 1 specifying how often to sample the goal point. Return: Randomly selected point as a Point(gennav.utils.geometry.Point). """ if random.random() > goal_sample_rate: new_point = Point() new_point.x = random.uniform(sample_area[0], sample_area[1]) new_point.y = random.uniform(sample_area[0], sample_area[1]) return new_point else: return goal
def plan(self, start, goal, env): """Plans path from start to goal avoiding obstacles. Args: start_point(gennav.utils.RobotState): with start point coordinates. end_point (gennav.utils.RobotState): with end point coordinates. env: (gennav.envs.Environment) Base class for an envrionment. Returns: gennav.utils.Trajectory: The planned path as trajectory dict: Dictionary containing additional information like the node_list """ # Check if start and goal states are obstacle free if not env.get_status(start): raise InvalidStartState(start, message="Start state is in obstacle.") if not env.get_status(goal): raise InvalidGoalState(goal, message="Goal state is in obstacle.") # Initialize start and goal nodes start_node = Node(state=start) goal_node = Node(state=goal) # Initialize node_list with start node_list = [start_node] # Loop to keep expanding the tree towards goal if there is no direct connection traj = Trajectory(path=[start, goal]) if not env.get_traj_status(traj): while True: # Sample random state in specified area rnd_state = self.sampler() # Find nearest node to the sampled point distance_list = [ compute_distance(node.state.position, rnd_state.position) for node in node_list ] try: # for python2 nearest_node_index = min(xrange(len(distance_list)), key=distance_list.__getitem__) except NameError: # for python 3 nearest_node_index = distance_list.index( min(distance_list)) nearest_node = node_list[nearest_node_index] # Create new point in the direction of sampled point theta = math.atan2( rnd_state.position.y - nearest_node.state.position.y, rnd_state.position.x - nearest_node.state.position.x, ) new_point = Point() new_point.x = (nearest_node.state.position.x + self.expand_dis * math.cos(theta)) new_point.y = (nearest_node.state.position.y + self.expand_dis * math.sin(theta)) # Check whether new point is inside an obstacles if not env.get_status(RobotState(position=new_point)): continue else: new_node = Node.from_coordinates(new_point) new_node.parent = nearest_node node_list.append(new_node) # Check if goal has been reached or if there is direct connection to goal distance_to_goal = compute_distance(goal_node.state.position, new_node.state.position) traj = Trajectory(path=[RobotState(position=new_point), goal]) if distance_to_goal < self.expand_dis or env.get_traj_status( traj): goal_node.parent = node_list[-1] node_list.append(goal_node) print("Goal reached!") break else: goal_node.parent = start_node node_list = [start_node, goal_node] # Construct path by traversing backwards through the tree path = [] last_node = node_list[-1] while last_node.parent is not None: path.append(last_node.state) last_node = last_node.parent path.append(start) info_dict = {} info_dict["node_list"] = node_list path = Trajectory(path[::-1]) if len(path.path) == 1: raise PathNotFound(path, message="Path contains only one state") return (path, info_dict)
def plan(self, start, goal, env): """Plans path from start to goal avoiding obstacles. Args: start_point(gennav.utils.RobotState): with start point coordinates. end_point (gennav.utils.RobotState): with end point coordinates. env: (gennav.envs.Environment) Base class for an envrionment. Returns: gennav.utils.Trajectory: The planned path as trajectory """ # Initialize start and goal nodes start_node = Node(state=start) goal_node = Node(state=goal) # Initialize node_list with start node_list = [start_node] # Loop to keep expanding the tree towards goal if there is no direct connection traj = Trajectory(path=[start, goal]) if not env.get_traj_status(traj): while True: # Sample random point in specified area rnd_point = self.sampler(self.sample_area, goal.position, self.goal_sample_rate) # Find nearest node to the sampled point distance_list = [(node.state.position.x - rnd_point.x)**2 + (node.state.position.y - rnd_point.y)**2 + (node.state.position.z - rnd_point.z)**2 for node in node_list] try: # for python2 nearest_node_index = min(xrange(len(distance_list)), key=distance_list.__getitem__) except NameError: # for python 3 nearest_node_index = distance_list.index( min(distance_list)) nearest_node = node_list[nearest_node_index] # Create new point in the direction of sampled point theta = math.atan2( rnd_point.y - nearest_node.state.position.y, rnd_point.x - nearest_node.state.position.x, ) new_point = Point() new_point.x = (nearest_node.state.position.x + self.expand_dis * math.cos(theta)) new_point.y = (nearest_node.state.position.y + self.expand_dis * math.sin(theta)) # Check whether new point is inside an obstacles if not env.get_status(RobotState(position=new_point)): continue else: new_node = Node.from_coordinates(new_point) new_node.parent = nearest_node node_list.append(new_node) # Check if goal has been reached or if there is direct connection to goal del_x, del_y, del_z = ( new_node.state.position.x - goal_node.state.position.x, new_node.state.position.y - goal_node.state.position.y, new_node.state.position.z - goal_node.state.position.z, ) distance_to_goal = math.sqrt(del_x**2 + del_y**2 + del_z**2) traj = Trajectory(path=[RobotState(position=new_point), goal]) if distance_to_goal < self.expand_dis or env.get_traj_status( traj): goal_node.parent = node_list[-1] node_list.append(goal_node) print("Goal reached!") break else: goal_node.parent = start_node node_list = [start_node, goal_node] # Construct path by traversing backwards through the tree path = [] last_node = node_list[-1] while node_list[node_list.index(last_node)].parent is not None: node = node_list[node_list.index(last_node)] path.append(RobotState(position=node.state.position)) last_node = node.parent path.append(start) path = Trajectory(list(reversed(path))) return path