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 construct(self, env): """Constructs DStar graph. Args: env (gennav.envs.Environment): Base class for an envrionment. Returns: graph (gennav.utils.graph): A dict where the keys correspond to nodes and the values for each key is a list of the neighbour nodes """ nodes = [] graph = Graph() i = 0 # samples points from the sample space until n points # outside obstacles are obtained while i < self.n: sample = self.sampler() if not env.get_status(sample): continue else: i += 1 node = NodeDstar(state=sample) nodes.append(node) # finds neighbours for each node in a fixed radius r for node1 in nodes: for node2 in nodes: if node1 != node2: dist = compute_distance(node1.state.position, node2.state.position) if dist < self.r: if env.get_traj_status(Trajectory([node1.state, node2.state])): if node1 not in graph.nodes: graph.add_node(node1) if node2 not in graph.nodes: graph.add_node(node2) if ( node2 not in graph.edges[node1] and node1 not in graph.edges[node2] ): graph.add_edge( node1, node2, ) return graph
def msg_to_traj(msg): """Converts the trajectory containing ROS message to gennav.utils.Trajectory data type Args: msg (geometry_msgs/MultiDOFJointTrajectory.msg): The ROS message Returns: gennav.utils.Trajectory : Object containing the trajectory data """ path = [] timestamps = [] for point in msg.points: timestamps.append(point.time_from_start) position = utils.common.Point( x=point.transforms[0].translation.x, y=point.transforms[0].translation.y, z=point.transforms[0].translation.z, ) rotation = tf.transformations.euler_from_quaternion([ point.transforms[0].rotation.x, point.transforms[0].rotation.y, point.transforms[0].rotation.z, point.transforms[0].rotation.w, ]) rotation = utils.geometry.OrientationRPY(roll=rotation[0], pitch=rotation[1], yaw=rotation[2]) linear_vel = utils.geometry.Vector3D( x=point.velocities[0].linear.x, y=point.velocities[0].linear.y, z=point.velocities[0].linear.z, ) angular_vel = utils.geometry.Vector3D( x=point.velocities[0].angular.x, y=point.velocities[0].angular.y, z=point.velocities[0].linear.z, ) velocity = Velocity(linear=linear_vel, angular=angular_vel) state = RobotState(position=position, orientation=rotation, velocity=velocity) path.append(state) traj = Trajectory(path=path, timestamps=timestamps) return traj
def construct(self, env): """Constructs PRM-Star graph. Args: env (gennav.envs.Environment): Base class for an envrionment. Returns: graph (dict): A dict where the keys correspond to nodes and the values for each key is a list of the neighbour nodes """ nodes = [] graph = {} i = 0 # samples points from the sample space until n points # outside obstacles are obtained while i < self.n: sample = self.sampler(self.sample_area) if not env.get_status(RobotState(position=sample)): continue else: i += 1 nodes.append(sample) # finds neighbours for each node in a dynamic radius for node1 in nodes: for node2 in nodes: if node1 != node2: r = self.c * math.sqrt(math.log(self.n) / self.n) dist = math.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2) if dist < r: traj = Trajectory([ RobotState(position=node1), RobotState(position=node2) ]) if env.get_traj_status(traj): if node1 not in graph: graph[node1] = [node2] elif node2 not in graph[node1]: graph[node1].append(node2) if node2 not in graph: graph[node2] = [node1] elif node1 not in graph[node2]: graph[node2].append(node1) return graph
def plan(self, start, goal, env): """Plans the path to be taken by the robot to get from start to goal in the form of waypoints Args: start (gennav.utils.RobotState) : starting state of the robot (x, y and z coordinates) goal (gennav.utils.RobotState) : goal state of the robot (x, y and z coordinates) env (gennav.envs.Environment) : Type of environment the robot is operating in Returns: trajectory (gennav.utils.Trajectory) : A list of waypoints(in the form of robot states) that the robot will follow to go to the goal from the start 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.") self.current = start self.goal = goal self.env = env waypoints = [self.current] while (math.sqrt((self.current.position.x - self.goal.position.x)**2 + (self.current.position.y - self.goal.position.y)**2) > self.error): grad_attract = self.grad_attractive() grad_repel = self.grad_repulsive() grad = [0, 0] grad[0] = grad_attract[0] + grad_repel[0] grad[1] = grad_attract[1] + grad_repel[1] self.current.position.x = self.current.position.x - self.STEP_SIZE * grad[ 0] self.current.position.y = self.current.position.y - self.STEP_SIZE * grad[ 1] waypoints.append(self.current) trajectory = Trajectory(waypoints) if len(trajectory.path) == 1: raise PathNotFound(trajectory, message="Path contains only one state") return trajectory, {}
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 replan(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 """ global traj, open_, graph, closed min_dist = float("inf") old_traj = traj for node in old_traj.path: dist = compute_distance(node.position, start.position) traj_chk = Trajectory([node, start]) if dist < min_dist and (env.get_traj_status(traj_chk)): min_dist = dist s = node a = old_traj.path.index(s) new_traj = Trajectory([start]) new_traj.path.extend(old_traj.path[a:]) if env.get_traj_status(new_traj): return new_traj print 1 min_dist = float("inf") for node in graph.nodes: dist = compute_distance(node.state.position, start.position) traj = Trajectory([node.state, start]) if dist < min_dist and (env.get_traj_status(traj)): min_dist = dist print 2 start_node = node # find collision free point in graph closest to end_point min_dist = float("inf") for node in graph.nodes: dist = compute_distance(node.state.position, goal.position) traj = Trajectory([node.state, goal]) if dist < min_dist and (env.get_traj_status(traj)): min_dist = dist print 3 goal_node = node found=0 for state in traj.path: if not env.get_status(state): print 4 for item in graph.nodes: if item.state == state: node_ = item break node_.t = float("inf") if node_.tag == "CLOSED": print 4 closed.remove(node_) node_.tag = "OPEN" open_.append(node) else: print 999 found=1 break if found==0: for state in traj.path: if traj.path.index(state) < len(traj.path) - 1 and ( not env.get_traj_status( Trajectory([state, traj.path[traj.path.index(state) + 1]]) ) ): print 55 next_state=traj.path[traj.path.index(state) + 1] ##unable to find next_state in graph.nodes for item in graph.nodes: if item.state == next_state: print 444 node_ = item break node_.t = float("inf") if node_.tag == "CLOSED": print 5 closed.remove(node_) node_.tag = "OPEN" open_.append(node) else: print 999 found=1 break else: print 333 if found==1: if node_.h is not None: for neighbour in graph.edges[node_]: if neighbour.tag == "CLOSED": print 6 closed.remove(neighbour) neighbour.tag = "OPEN" open_.append(neighbour) elif neighbour.tag == "NEW": print 60 neighbour.parent=node neighbour.h=compute_distance( node_.state.position, neighbour.state.position )+node_.h neighbour.k=neighbour.h neighbour.tag = "OPEN" open_.append(neighbour) while len(open_) > 0: open_.sort() current_node = open_.pop(0) current_node.tag = "CLOSED" closed.append(current_node) if current_node.k >= start_node.h and start_node.tag == "CLOSED": print 7 current_node = start_node path = [] path.append(start) # while current_node.parent is not None: # print 1 # path.append(current_node.state) # h_ = float("inf") # neighbours = graph.edges[current_node] # for neighbour in neighbours: # if neighbour.h < h_: # node_ = neighbour # h_ = neighbour.h # current_node = node_ i = 0 while current_node.parent is not None: path.append(current_node.state) current_node = current_node.parent if i > 40: traj = Trajectory(path) print 88 return traj i += 1 path.append(goal_node.state) path.append(goal) traj = Trajectory(path) print 9 return traj if current_node.k < current_node.h and current_node.k is not None: print 10 for neighbour in graph.edges[current_node]: if ( neighbour.tag != "NEW" and neighbour.h <= current_node.k and current_node.h > neighbour.h + c(current_node, neighbour) ): current_node.parent = neighbour current_node.h = neighbour.h + c(current_node, neighbour) if current_node.k == current_node.h and current_node.k is not None: print 11 for neighbour in graph.edges[current_node]: if ( neighbour.tag == "NEW" or ( neighbour.parent == current_node and neighbour.h != current_node.h + c(current_node, neighbour) ) or ( neighbour.parent != current_node and neighbour.h > current_node.h + c(current_node, neighbour) ) ): neighbour.parent = current_node insert(neighbour, current_node.h + c(current_node, neighbour)) elif current_node.k is not None: print 12 for neighbour in graph.edges[current_node]: if neighbour.tag == "NEW" or ( neighbour.parent == current_node and neighbour.h != current_node.h + c(current_node, neighbour) ): neighbour.parent = current_node insert(neighbour, current_node.h + c(current_node, neighbour)) elif ( neighbour.parent != current_node and neighbour.h > current_node.h + c(current_node, neighbour) ): insert(current_node, current_node.h) elif ( neighbour.parent != current_node and current_node.h > neighbour.h + c(current_node, neighbour) and (neighbour.tag == "CLOSED") and neighbour.h > current_node.k ): insert(neighbour, neighbour.h) path = [start] traj = Trajectory(path) raise PathNotFound(traj, message="Path contains only one state")
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 global graph, traj 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.state.position, start.position) traj = Trajectory([node.state, start]) if dist < min_dist and (env.get_traj_status(traj)): min_dist = dist start_node = node # find collision free point in graph closest to end_point min_dist = float("inf") for node in graph.nodes: dist = compute_distance(node.state.position, goal.position) traj = Trajectory([node.state, goal]) if dist < min_dist and (env.get_traj_status(traj)): min_dist = dist goal_node = node global open_, closed open_ = [] closed = [] goal_node.h = 0 goal_node.k = 0 open_.append(goal_node) while len(open_) > 0: open_.sort() current_node = open_.pop(0) current_node.tag = "CLOSED" closed.append(current_node) if current_node.state.position == start_node.state.position: path = [] path.append(start) # while current_node.parent is not None: # print 1 # path.append(current_node.state) # h_=float("inf") # neighbours=graph.edges[current_node] # for neighbour in neighbours: # if neighbour.h<h_: # node=neighbour # h_=neighbour.h # current_node=node while current_node.parent is not None: path.append(current_node.state) current_node = current_node.parent path.append(goal_node.state) path.append(goal) traj = Trajectory(path) return traj neighbours = graph.edges[current_node] for neighbour in neighbours: if neighbour.tag=="CLOSED": continue neighbour.parent = current_node neighbour.h = compute_distance( current_node.state.position, neighbour.state.position )+current_node.h neighbour.k = neighbour.h neighbour.tag = "OPEN" open_.append(neighbour) path = [start] traj = Trajectory(path) raise PathNotFound(traj, message="Path contains only one state")
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): """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)
def astar(graph, start, end, heuristic={}): """ Performs A-star search to find the shortest path from start to end Args: graph (gennav.utils.graph): Dictionary representing the graph where keys are the nodes and the value is a list of all neighbouring nodes start (gennav.utils.RobotState): Point representing key corresponding to the start point end (gennav.utils.RobotState): Point representing key corresponding to the end point heuristic (dict): Dictionary containing the heuristic values for all the nodes, if not specified the default heuristic is euclidean distance Returns: gennav.utils.Trajectory: The planned path as trajectory """ if not (start in graph.nodes and end in graph.nodes): path = [start] traj = Trajectory(path) return traj open_ = [] closed = [] # calculates heuristic for start if not provided by the user # pushes the start point in the open_ Priority Queue start_node = NodeAstar(state=start) if len(heuristic) == 0: start_node.h = compute_distance(start.position, end.position) else: start_node.h = heuristic[start] start_node.g = 0 start_node.f = start_node.g + start_node.h open_.append(start_node) # performs astar search to find the shortest path while len(open_) > 0: open_.sort() current_node = open_.pop(0) closed.append(current_node) # checks if the goal has been reached if current_node.state.position == end.position: path = [] # forms path from closed list while current_node.parent is not None: path.append(current_node.state) current_node = current_node.parent path.append(start_node.state) # returns reversed path path = path[::-1] traj = Trajectory(path) return traj # continues to search for the goal # makes a list of all neighbours of the current_node neighbours = graph.edges[current_node.state] # adds them to open_ if they are already present in open_ # checks and updates the total cost for all the neighbours for node in neighbours: # creates neighbour which can be pushed to open_ if required neighbour = NodeAstar(state=node, parent=current_node) # checks if neighbour is in closed if neighbour in closed: continue # calculates weight cost neighbour.g = compute_distance(node.position, current_node.state.position) # calculates heuristic for the node if not provided by the user if len(heuristic) == 0: neighbour.h = compute_distance(node.position, end.position) else: neighbour.h = heuristic[node] # calculates total cost neighbour.f = neighbour.g + neighbour.h # checks if the total cost of neighbour needs to be updated # if it is presnt in open_ else adds it to open_ flag = 1 for new_node in open_: if neighbour == new_node and neighbour.f < new_node.f: new_node = neighbour # lgtm [py/multiple-definition] flag = 0 break elif neighbour == new_node and neighbour.f > new_node.f: flag = 0 break if flag == 1: open_.append(neighbour) # if path doesn't exsist it raises a PathNotFound Exception. path = [start] traj = Trajectory(path) raise PathNotFound(traj, message="Path contains only one state")
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