def test_dstar(): obstacles = [[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]] sampler = UniformRectSampler(-5, 15, -5, 15) poly = PolygonEnv() start = RobotState(position=Point(0, 0)) goal = RobotState(position=Point(12, 10)) my_tree = DStar(sampler=sampler, r=3, n=75) poly.update(obstacles) path = my_tree.plan(start, goal, poly) from gennav.envs.common import visualize_path visualize_path(path, poly) obstacles = [ [(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)], [(10, 8), (12, 8), (11, 6)], ] # obstacles = [[(10, 7), (9, 10), (4, 11), (5, 7)], [(5, 5), (5, 7), (7, 7), (7, 5)],[(9,8),(13,8),(11,5)]] poly.update(obstacles) path_new = my_tree.replan(start, goal, poly) from gennav.envs.common import visualize_path visualize_path(path_new, poly)
def test_prm_plan(): general_obstacles_list = [ [[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]], [ [(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)], [(4, 0), (4, 5), (5, 5), (5, 0)], [(8, 2), (8, 7), (10, 7), (10, 2)], ], ] sampler = UniformRectSampler(-5, 15, -5, 15) poly = PolygonEnv(buffer_dist=0.1) start = RobotState(position=Point(0, 0)) goal = RobotState(position=Point(12, 10)) my_tree = PRM(sampler=sampler, r=5, n=100) for obstacles in general_obstacles_list: poly.update(obstacles) path, _ = my_tree.plan(start, goal, poly) # from gennav.envs.common import visualize_path # visualize_path(path, poly) if len(path.path) != 1: assert poly.get_traj_status(path) is True
def test_rrt(): general_obstacles_list = [ [[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]], [ [(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)], [(4, 0), (4, 5), (5, 5), (5, 0)], [(8, 2), (8, 7), (10, 7), (10, 2)], ], ] poly = PolygonEnv() # poly.update(general_obstacles_list) for obstacles in general_obstacles_list: poly.update(obstacles) # Instatiate rrt planner object my_tree = RRT(sample_area=(-5, 15), sampler=sampler, expand_dis=0.1) start = RobotState(position=Point(1, 1)) goal = RobotState(position=Point(10, 10)) path = my_tree.plan(start, goal, poly) # from gennav.envs.common import visualize_path # visualize_path(path, poly) assert poly.get_traj_status(path) is True
def prmstar_plan(): general_obstacles_list = [ [[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]], [ [(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)], [(4, 0), (4, 5), (5, 5), (5, 0)], [(8, 2), (8, 7), (10, 7), (10, 2)], ], ] sampler = UniformRectSampler(-5, 15, -5, 15) polygon = PolygonEnv() start = RobotState(position=Point(0, 0)) goal = RobotState(position=Point(12, 10)) my_prmstar = PRMStar(sampler=sampler, c=30, n=100) # Plan a path using the PRMStar.plan method for each obstacle: for obstacles in general_obstacles_list: polygon.update(obstacles) # updates the environment with the obstacle path = my_prmstar.plan(start, goal, polygon) visualize_path(path, polygon) if len(path.path) != 1: # check if the path has only the start state assert polygon.get_traj_status(path) is True
def test_rrtstar(): general_obstacles_list = [ [ (1, 1), (2, 1), (2, 2), (1, 2), ], [(3, 4), (4, 4), (4, 5), (3, 5)], ] poly = PolygonEnv(buffer_dist=0.1) poly.update(general_obstacles_list) sampler = UniformRectSampler(0, 6, 0, 6) my_tree = RRTstar( sampler=sampler, expand_dis=0.1, neighbourhood_radius=0.5, goal_distance=0.5, max_iter=2000, ) start = RobotState(position=Point(0, 0)) goal = RobotState(position=Point(5, 5)) path, _ = my_tree.plan(start, goal, poly) # from gennav.utils.visualisation import visualize_node # node_list = _['node_list'] # visualize_node(node_list, poly) # from gennav.envs.common import visualize_path # visualize_path(path, poly) if len(path.path) != 1: assert poly.get_traj_status(path) is True
def test_rrt(): general_obstacles_list = [ [[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]], [ [(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)], [(4, 0), (4, 5), (5, 5), (5, 0)], [(8, 2), (8, 7), (10, 7), (10, 2)], ], ] sampler = UniformRectSampler(-5, 15, -5, 15) poly = PolygonEnv(buffer_dist=0.5) my_tree = RRT(sampler=sampler, expand_dis=0.1) start = RobotState(position=Point(1, 1)) goal = RobotState(position=Point(10, 10)) for obstacles in general_obstacles_list: poly.update(obstacles) path, _ = my_tree.plan(start, goal, poly) # from gennav.utils.visualisation import visualize_node # node_list = _['node_list'] # visualize_node(node_list, poly) # from gennav.envs.common import visualize_path # visualize_path(path, poly) assert poly.get_traj_status(path) is True
def test_potential_field(): obstacle_list = [[(1, 1), (1.5, 1.5), (1.5, 1)]] env = PolygonEnv(buffer_dist=0.1) env.update(obstacle_list) my_planner = PotentialField(KP=5, ETA=100, THRESH=15, STEP_SIZE=0.1, error=0.2) start = RobotState(position=Point(0, 0)) goal = RobotState(position=Point(7, 7)) path, _ = my_planner.plan(start, goal, env) assert env.get_traj_status(path) is True
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: 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 __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 transform(self, frame1, frame2, rsf1): """Transform robotPose from one pose to the other Args: frame1 (string) : from the frame (world, bot, map) frame2 (string) : to the frame (world, bot, map) rsf1 (gennav.utils.common.RobotState) : RobotState in frame1 Returns: rsf2 (gennav.utils.common.RobotState) : RobotState in frame2 """ # TODO: Make it more robust in terms of checking frames # Check if the required trnasform or the inverse of the transform exists frame = frame2 + "T" + frame1 frame_inv = frame1 + "T" + frame2 if frame in self.transforms.keys(): t_matrix = self.transforms[frame]["transform"] elif frame_inv in self.transforms.keys(): t_matrix = np.linalg.inv(self.transforms[frame_inv]["transform"]) else: raise Exception("Transform for the frames not found") # Transform using matrix multiplication pf2 = np.dot( t_matrix, np.array([rsf1.position.x, rsf1.position.y, 1]).reshape(3, 1)) rsf2 = RobotState(position=Point(pf2[0].item(), pf2[1].item())) # Return RobotState return rsf2
def fillOccupancy(self): """Function that fill the occupnacy grid on every update Assumptions: 1. RobotPose is considered (0, 0, 0) to accomodate the laser scan, which produces ranges wrt to the bot 2. The RobotPose in the occupancy grid is (X * scale_factor/2, Y * scale_factor /2, 0) 3. The attribute robotPose is the real pose of the robot wrt to the world Frame, thus it helps us to calculate the transform for trajectory and pose validity queries """ self.grid[:] = 0 ang_min, ang_max, ranges = self.scan angle_step = (ang_max - ang_min) / len(ranges) for i, rng in enumerate(ranges): # Check for obstacles if np.abs(rng) is not np.inf: x, y = ( rng * np.cos(ang_min + i * angle_step), rng * np.sin(ang_max + i * angle_step), ) newState = self.transform("bot", "map", RobotState(Point(x, y, 0))) x_, y_ = newState.position.x, newState.position.y # Checking if the range is within the grid, to mark them as occupied if 0 <= x_ < self.grid.shape[0] and 0 <= y_ < self.grid.shape[ 1]: if self.grid[int(x_)][int(-y_ - 1)] != 1: self.grid[int(x_)][int(-y_ - 1)] = 1
def test_uniform_circular_sampler(): radius = 5 centre = Point(x=1, y=2) sampler = UniformCircularSampler(radius, centre) for _ in range(100): s = sampler() p = s.position assert compute_distance(p, centre) <= radius
def grad_repulsive(self): """Calculates the gradient due to the repulsive component of the potential field, i.e, the potential field created by the obstacles Args: None Returns: total_grad (float) : Total gradient due to potential field of all obstacles at the state of the robot """ total_grad = [0.0, 0.0] min_dist = self.env.minimum_distances(self.current) for i, _ in enumerate(self.env.obstacle_list): if min_dist[i] < self.THRESH: dummy_state_x = RobotState( position=Point( self.current.position.x + 0.01, self.current.position.y, self.current.position.z, ) ) x_grad = self.env.minimum_distances(dummy_state_x)[i] dummy_state_y = RobotState( position=Point( self.current.position.x, self.current.position.y + 0.01, self.current.position.z, ) ) y_grad = self.env.minimum_distances(dummy_state_y)[i] grad = [ (x_grad - min_dist[i]) / 0.01, (y_grad - min_dist[i]) / 0.01, ] factor = ( self.ETA * ((1 / self.THRESH) - 1 / min_dist[i]) * 1 / (min_dist[i] ** 2 + 1) ) grad = [x * factor for x in grad] total_grad[0] = total_grad[0] + grad[0] total_grad[1] = total_grad[1] + grad[1] return total_grad
def __init__(self, radius, centre=Point(), goal=None, goal_sample_p=0): super(UniformCircularSampler, self).__init__() self.centre = centre self.r = radius self.goal = goal self.goal_sample_p = goal_sample_p if goal is None and goal_sample_p > 0: raise SamplingFailed( goal, message="goal must be specified if goal_sample_p > 0")
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 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_prm_plan(): general_obstacles_list = [ [[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]], [ [(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)], [(4, 0), (4, 5), (5, 5), (5, 0)], [(8, 2), (8, 7), (10, 7), (10, 2)], ], ] poly = PolygonEnv() for obstacles in general_obstacles_list: poly.update(obstacles) # Instatiate prm constructer object start = RobotState(position=Point(0, 0)) goal = RobotState(position=Point(12, 10)) my_tree = PRM(sample_area=(-5, 15), sampler=sampler, r=5, n=100) path = my_tree.plan(start, goal, poly) # from gennav.envs.common import visualize_path # visualize_path(path, poly) assert poly.get_traj_status(path) is True
def plan(self, start, goal, env): """Path planning method Args: start (gennav.utils.common.RobotState): Starting state. goal (gennav.utils.common.RobotState): Destination state env (gennav.envs.Environment): Environment object Returns: gennav.utils.Trajectory: The path as a 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. x_start = Node(state=start) x_goal = Node(state=goal) # Starting the tree: node_list = [x_start] # X_soln contains all the nodes near the goal X_soln = [] for i in range(self.max_iter): # defining the best cost as cbest if len(X_soln) == 0: cbest = float("inf") else: cbest = X_soln[0].cost + compute_distance( X_soln[0].state.position, x_goal.state.position ) for node in X_soln: if ( node.cost + compute_distance(node.state.position, x_goal.state.position) < cbest ): cbest = node.cost + compute_distance( node.state.position, x_goal.state.position ) # sample the new random point using the sample function x_rand = self.rectangular_sampler() x_rand_node = Node(state=x_rand) # Finding the nearest node to the random point distance_list = [ compute_distance(node.state.position, x_rand_node.state.position) for node in node_list ] min_dist = distance_list[0] min_index = 0 for index, distance in enumerate(distance_list): if distance < min_dist: min_dist = distance min_index = index x_nearest = node_list[min_index] # Steering at a distance of self.expand_dis theta = compute_angle(x_nearest.state.position, x_rand_node.state.position) x = x_nearest.state.position.x + self.expand_dis * math.cos(theta) y = x_nearest.state.position.y + self.expand_dis * math.sin(theta) t = RobotState(position=Point(x, y)) x_new = Node(state=t) # Defining a temporary trajectory between the new node and its nearest node traj = Trajectory(path=[x_nearest.state, x_new.state]) # Checking whether new node and the nearest node can be connected if env.get_traj_status(traj): # Adding new node to the node list node_list.append(x_new) # Defining the neighbours of x_new X_near = [] for node in node_list: if (compute_distance(node.state.position, x_new.state.position)) < ( self.neighbourhood_radius ) and ( # If it is inside the neighbourhood radius compute_distance(node.state.position, x_new.state.position) != 0 ): X_near.append(node) x_min = x_nearest c_min = x_min.cost + self.expand_dis for x_near in X_near: c_new = x_near.cost + compute_distance( x_near.state.position, x_new.state.position ) if c_new < c_min: traj = Trajectory(path=[x_near.state, x_new.state]) if env.get_traj_status(traj): x_min = x_near c_min = c_new # Defining the parent node and cost of x_new x_new.parent = x_min x_new.cost = c_min # Rewiring for x_near in X_near: c_near = x_near.cost c_new = x_new.cost + compute_distance( x_near.state.position, x_new.state.position ) if c_new < c_near: traj = Trajectory(path=[x_new.state, x_near.state]) if env.get_traj_status(traj): x_near.parent = x_new x_near.cost = c_new goal_traj = Trajectory(path=[x_new.state, x_goal.state]) if ( compute_distance(x_goal.state.position, x_new.state.position) ) < self.goal_distance and env.get_traj_status(goal_traj): X_soln.append(x_new) # X_soln contains all the solutions if len(X_soln) == 0: print("No solution found!") path = [] path.append(x_start.state) traj = Trajectory(path=path) info_dict = {} info_dict["node_list"] = node_list if len(traj.path) == 1: raise PathNotFound(path, message="Path contains only one state") return (traj, info_dict) else: print("Goal reached!") Cbest = X_soln[0].cost Xbest = X_soln[0] node_list.append(x_goal) for node in X_soln: if node.cost < Cbest: Cbest = node.cost Xbest = node x_goal.parent = Xbest # Xbest is the optimum solution to the problem. # Back tracing the path: path = [] path.append(x_goal.state) path.append(Xbest.state) p = Xbest.parent while p is not None: path.append(p.state) p = p.parent path.append(x_start.state) path.reverse() # print("No. of solutions " + str(len(X_soln))) trajectory = Trajectory(path=path) info_dict = {} info_dict["node_list"] = node_list return (trajectory, 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 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)
raise NotImplementedError("Specified controller ", env_name, " is not implemented") # Get message type object msg_dtype_name = param_dict["msg_dtype_name"] if msg_dtype_name in msg_dtype_registry.keys(): msg_dtype = msg_dtype_registry[msg_dtype_name] else: raise NotImplementedError("Specified message type ", planner_name, " is not compatible") # Construct controller controller_node = gennav_ros.Controller(controller) # Construct commander replan_interval = rospy.Duration(param_dict["replan_interval"]) commander_node = gennav_ros.Commander( planner=planner, env=env, replan_interval=replan_interval, msg_dtype=msg_dtype, ) # Execute command goal_state = RobotState(position=Point(*goal)) start_state = RobotState(position=Point(*start)) commander_node.goto(goal_state, start_state) # ROS Magic (spin) rospy.spin()
def plan(self, start, goal, env): """Path planning method Args: start (gennav.utils.common.RobotState): Starting state. goal (gennav.utils.common.RobotState): Destination state env (gennav.envs.Environment): Environment object Returns: gennav.utils.Trajectory: The path as a 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. x_start = Node(state=start) x_goal = Node(state=goal) # Starting the tree: node_list = [x_start] # X_soln contains all the nodes near the goal X_soln = [] for i in range(self.max_iter): # defining the best cost as cbest if len(X_soln) == 0: cbest = float("inf") else: cbest = X_soln[0].cost + compute_distance( X_soln[0].state.position, x_goal.state.position) for node in X_soln: if (node.cost + compute_distance(node.state.position, x_goal.state.position) < cbest): cbest = node.cost + compute_distance( node.state.position, x_goal.state.position) # sample the new random point using the sample function if cbest < float("inf"): # Converting x_start and x_goal into arrays so that matrix calculations can be handled x_start_array = np.array( [x_start.state.position.x, x_start.state.position.y], dtype=np.float32, ).reshape(2, 1) x_goal_array = np.array( [x_goal.state.position.x, x_goal.state.position.y], dtype=np.float32).reshape(2, 1) cmin = np.linalg.norm((x_goal_array - x_start_array)) x_center = (x_start_array + x_goal_array) / 2 a1 = (x_goal_array - x_start_array) / cmin I1 = np.eye(len(x_start_array))[0].reshape( len(x_start_array), 1) M = np.dot(a1, I1.T) [u, s, v] = np.linalg.svd(M) # numpy returns the transposed value for v, so to convert it back v = v.T # Calculating the rotation to world frame matrix given by C = U*diag{1,....1.det(U)det(V)}*V.T where * denotes matrix multiplication diag = np.eye(2) diag[1, 1] = np.linalg.det(u) * np.linalg.det(v) C = np.dot(np.dot(u, diag), v.T) L = np.eye(2) L[0, 0] = cbest / 2 L[1, 1] = np.sqrt(cbest**2 - cmin**2) / 2 rand_state = self.circular_sampler() x_ball = np.array( [rand_state.position.x, rand_state.position.y], dtype=np.float32).reshape(2, 1) x_rand_array = np.dot(np.dot(C, L), x_ball) + x_center x_rand = RobotState() x_rand.position.x = x_rand_array[0, 0] x_rand.position.y = x_rand_array[1, 0] x_rand_node = Node(state=x_rand) else: x_rand = self.rectangular_sampler() x_rand_node = Node(state=x_rand) # Finding the nearest node to the random point distance_list = [ compute_distance(node.state.position, x_rand_node.state.position) for node in node_list ] min_dist = distance_list[0] min_index = 0 for index, distance in enumerate(distance_list): if distance < min_dist: min_dist = distance min_index = index x_nearest = node_list[min_index] # Steering at a distance of self.expand_dis theta = compute_angle(x_nearest.state.position, x_rand_node.state.position) x = x_nearest.state.position.x + self.expand_dis * math.cos(theta) y = x_nearest.state.position.y + self.expand_dis * math.sin(theta) t = RobotState(position=Point(x, y)) x_new = Node(state=t) # Defining a temporary trajectory between the new node and its nearest node traj = Trajectory(path=[x_nearest.state, x_new.state]) # Checking whether new node and the nearest node can be connected if env.get_traj_status(traj): # Adding new node to the node list node_list.append(x_new) # Defining the neighbours of x_new X_near = [] for node in node_list: if (compute_distance( node.state.position, x_new.state.position)) < ( self.neighbourhood_radius ) and ( # If it is inside the neighbourhood radius compute_distance(node.state.position, x_new.state.position) != 0): X_near.append(node) x_min = x_nearest c_min = x_min.cost + self.expand_dis for x_near in X_near: c_new = x_near.cost + compute_distance( x_near.state.position, x_new.state.position) if c_new < c_min: traj = Trajectory(path=[x_near.state, x_new.state]) if env.get_traj_status(traj): x_min = x_near c_min = c_new # Defining the parent node and cost of x_new x_new.parent = x_min x_new.cost = c_min # Rewiring for x_near in X_near: c_near = x_near.cost c_new = x_new.cost + compute_distance( x_near.state.position, x_new.state.position) if c_new < c_near: traj = Trajectory(path=[x_new.state, x_near.state]) if env.get_traj_status(traj): x_near.parent = x_new x_near.cost = c_new goal_traj = Trajectory(path=[x_new.state, x_goal.state]) if (compute_distance(x_goal.state.position, x_new.state.position) ) < self.goal_distance and env.get_traj_status(goal_traj): X_soln.append(x_new) # X_soln contains all the solutions if len(X_soln) == 0: print("No solution found!") path = [] path.append(x_start.state) traj = Trajectory(path=path) info_dict = {} info_dict["node_list"] = node_list if len(traj.path) == 1: raise PathNotFound(path, message="Path contains only one state") return (traj, info_dict) else: print("Goal reached!") Cbest = X_soln[0].cost Xbest = X_soln[0] node_list.append(x_goal) for node in X_soln: if node.cost < Cbest: Cbest = node.cost Xbest = node x_goal.parent = Xbest # Xbest is the optimum solution to the problem. # Back tracing the path: path = [] path.append(x_goal.state) path.append(Xbest.state) p = Xbest.parent while p is not None: path.append(p.state) p = p.parent path.append(x_start.state) path.reverse() # print("No. of solutions " + str(len(X_soln))) trajectory = Trajectory(path=path) info_dict = {} info_dict["node_list"] = node_list return (trajectory, info_dict)
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, {}
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
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)
UniformRectSampler, ) general_obstacles_list = [ [[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]], [ [(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)], [(4, 0), (4, 5), (5, 5), (5, 0)], [(8, 2), (8, 7), (10, 7), (10, 2)], ], ] # A list of obstacles for the planner. sampler = UniformRectSampler(-5, 15, -5, 15) # for uniformly sampling points poly = PolygonEnv( buffer_dist=1.0 ) # intializing the environment object needed for updating the obstacles of the environment my_tree = RRG(sampler=sampler, expand_dis=1.0, max_iter=500) # initializing RRG planner object start = RobotState(position=Point(0, 0)) # setting start point to (0,0) goal = RobotState(position=Point(10, 10)) # setting end point to (0,0) for obstacles in general_obstacles_list: # updating the environment with all obstacles poly.update(obstacles) path, _ = my_tree.plan(start, goal, poly) # planning the path visualize_path(path, poly) # visualizing path and obstacles