def test_astar_heuristic(): graph = Graph() node1 = RobotState(position=Point(0, 0)) node2 = RobotState(position=Point(1, 1)) node3 = RobotState(position=Point(3, 1)) node4 = RobotState(position=Point(2, 2)) node5 = RobotState(position=Point(3, 2)) node6 = RobotState(position=Point(2, 3)) graph.add_node(node1) graph.add_node(node2) graph.add_node(node3) graph.add_node(node4) graph.add_node(node5) graph.add_node(node6) graph.add_edge(node1, node2) graph.add_edge(node2, node3) graph.add_edge(node2, node4) graph.add_edge(node4, node5) graph.add_edge(node4, node6) graph.add_edge(node5, node6) heuristic = { node1: 5, node2: 3, node3: 1, node4: 1, node5: 0, node6: 2, } start = node1 end = node5 _ = astar(graph, start, end, heuristic) print(_.path)
def test_astar(): graph = Graph() node1 = RobotState(position=Point(0, 0)) node2 = RobotState(position=Point(1, 1)) node3 = RobotState(position=Point(3, 1)) node4 = RobotState(position=Point(2, 2)) node5 = RobotState(position=Point(3, 2)) node6 = RobotState(position=Point(2, 3)) graph.add_node(node1) graph.add_node(node2) graph.add_node(node3) graph.add_node(node4) graph.add_node(node5) graph.add_node(node6) graph.add_edge(node1, node2) graph.add_edge(node2, node3) graph.add_edge(node2, node4) graph.add_edge(node4, node5) graph.add_edge(node4, node6) graph.add_edge(node5, node6) start = node1 end = node5 _ = astar(graph, start, end) print(_.path)
def test_astar(): graph = { Point(0, 0): [Point(1, 1)], Point(1, 1): [Point(0, 0), Point(3, 1), Point(2, 2)], Point(3, 1): [Point(1, 1)], Point(2, 2): [Point(1, 1), Point(3, 2), Point(2, 3)], Point(3, 2): [Point(2, 2), Point(2, 3)], Point(2, 3): [Point(2, 2), Point(3, 2)], } start = Point(0, 0) end = Point(3, 2) _ = astar(graph, start, end)
def plan(self, start, goal, env): """Constructs a graph avoiding obstacles and then plans path from start to goal within the graph. Args: start (gennav.utils.RobotState): tuple with start point coordinates. goal (gennav.utils.RobotState): tuple with end point coordinates. env (gennav.envs.Environment): Base class for an envrionment. Returns: gennav.utils.Trajectory: The planned path as trajectory """ # construct graph graph = self.construct(env) # find collision free point in graph closest to start_point min_dist = float("inf") for node in graph.keys(): dist = math.sqrt((node.x - start.position.x)**2 + (node.y - start.position.y)**2) traj = Trajectory([ RobotState(position=node), RobotState(position=start.position) ]) if dist < min_dist and (env.get_traj_status(traj)): min_dist = dist s = node # find collision free point in graph closest to end_point min_dist = float("inf") for node in graph.keys(): dist = math.sqrt((node.x - goal.position.x)**2 + (node.y - goal.position.y)**2) traj = Trajectory([ RobotState(position=node), RobotState(position=goal.position) ]) if dist < min_dist and (env.get_traj_status(traj)): min_dist = dist e = node # add start_point to path path = [start] traj = Trajectory(path) # perform astar search p = astar(graph, s, e) if len(p.path) == 1: return traj else: traj.path.extend(p.path) # add end_point to path traj.path.append(goal) return traj
def plan(self, start, goal, env): """Constructs a graph avoiding obstacles and then plans path from start to goal within the graph. Args: start (gennav.utils.RobotState): tuple with start point coordinates. goal (gennav.utils.RobotState): tuple 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 """ # 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.") # construct graph graph = self.construct(env) # find collision free point in graph closest to start_point min_dist = float("inf") for node in graph.nodes: dist = compute_distance(node.position, start.position) traj = Trajectory([node, start]) if dist < min_dist and (env.get_traj_status(traj)): min_dist = dist s = node # find collision free point in graph closest to end_point min_dist = float("inf") for node in graph.nodes: dist = compute_distance(node.position, goal.position) traj = Trajectory([node, goal]) if dist < min_dist and (env.get_traj_status(traj)): min_dist = dist e = node # add start_point to path path = [start] traj = Trajectory(path) # perform astar search p = astar(graph, s, e) if len(p.path) == 1: raise PathNotFound(p, message="Path contains only one state") else: traj.path.extend(p.path) # add end_point to path traj.path.append(goal) return traj, {}
def test_astar_heuristic(): graph = { Point(0, 0): [Point(1, 1)], Point(1, 1): [Point(0, 0), Point(3, 1), Point(2, 2)], Point(3, 1): [Point(1, 1)], Point(2, 2): [Point(1, 1), Point(3, 2), Point(2, 3)], Point(3, 2): [Point(2, 2), Point(2, 3)], Point(2, 3): [Point(2, 2), Point(3, 2)], } heuristic = { Point(0, 0): 5, Point(1, 1): 3, Point(3, 1): 1, Point(2, 2): 1, Point(3, 2): 0, Point(2, 3): 2, } start = Point(0, 0) end = Point(3, 2) _ = astar(graph, start, end, heuristic)
def plan(self, start, end, env): """Plans path from start to goal avoiding obstacles. Args: start (gennav.utils.RobotState): Starting state end (gennav.utils.RobotState): Goal state env: (gennav.envs.Environment) Base class for an envrionment. Returns: gennav.utils.Trajectory: The planned path as trajectory dict: Dictionary containing additional information """ # Check if start and end states are obstacle free if not env.get_status(start): raise InvalidStartState(start, message="Start state is in obstacle.") if not env.get_status(end): raise InvalidGoalState(end, message="Goal state is in obstacle.") # Initialize graph graph = Graph() graph.add_node(start) # Initialize node list with Starting Position node_list = [start] # Loop for maximum iterations to get the best possible path for iter in range(self.max_iter): rnd_node = self.sampler() # Find nearest node to the sampled point distance_list = [ compute_distance(node.position, rnd_node.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_node.position.y - nearest_node.position.y, rnd_node.position.x - nearest_node.position.x, ) new_node = RobotState(position=Point( nearest_node.position.x + self.exp_dis * math.cos(theta), nearest_node.position.y + self.exp_dis * math.sin(theta), )) # Check whether new point is inside an obstacles trj = Trajectory([nearest_node, new_node]) if not env.get_status(new_node) or not env.get_traj_status(trj): continue else: node_list.append(new_node) # FINDING NEAREST INDICES nnode = len(node_list) + 1 # The circle in which to check parent node and rewiring r = self.circle * math.sqrt((math.log(nnode) / nnode)) dist_list = [ compute_distance(node.position, new_node.position) for node in node_list ] # Getting all the indexes within r units of new_node near_inds = [dist_list.index(i) for i in dist_list if i <= r**2] graph.add_node(new_node) graph.add_edge(nearest_node, new_node) # Add all the collision free edges to the graph. for ind in near_inds: node_check = node_list[ind] trj = Trajectory([new_node, node_check]) # If the above condition is met append the edge. if env.get_traj_status(trj): graph.add_edge(new_node, node_check) if end not in graph.nodes: min_cost = 2.0 closest_node = RobotState() for node in graph.nodes: temp = compute_distance(node.position, end.position) if env.get_traj_status(Trajectory([node, end ])) and temp < min_cost: min_cost = temp closest_node = node graph.add_edge(closest_node, end) graph.add_node(end) if start in graph.edges[end]: graph.del_edge(start, end) path = [] p = astar(graph, start, end) path.extend(p.path) if len(path) != 1: print("Goal Reached!") path = Trajectory(path) if len(path.path) == 1: raise PathNotFound(path, message="Path contains only one state") return path, {}