def __init__(self, fullMap, robot, plotter=None, dipController=None): """ An RRT path planner that takes into account orientation requirements. Note: Dipoles and poses are always numpy arrays with x, y, and theta. :param fullMap: A RRTMap object :param robot: A robotRRT object :param plotter: A RRTPlotter object. If plotting is desired :param dipController: A DipolarController object """ # TODO: CHECK TO SEE WHICH SHOULD BE KEPT HERE # DEBUGING TOOLS self.DEBUGER = False # Debugger is on self.PLOT_TREE = False # Plot RRT tree as it is grown self.PLOT_TREE_FAIL = False # Plot RRT tree in case of timeout # Settings self.connectDist = 1 # Euclidean distance between nodes for CONNECT self.closeEnoughDist = .25 self.closeEnoughAng = .5 self.fullMap = fullMap self.robot = robot if dipController is None: self.dipControl = DipolarController(k1=.5, k2=1.5, lambdaValue=3) else: self.dipControl = dipController self.plotter = plotter
class DipolarRRT: def __init__(self, fullMap, robot, plotter=None, dipController=None): """ An RRT path planner that takes into account orientation requirements. Note: Dipoles and poses are always numpy arrays with x, y, and theta. :param fullMap: A RRTMap object :param robot: A robotRRT object :param plotter: A RRTPlotter object. If plotting is desired :param dipController: A DipolarController object """ # TODO: CHECK TO SEE WHICH SHOULD BE KEPT HERE # DEBUGING TOOLS self.DEBUGER = False # Debugger is on self.PLOT_TREE = False # Plot RRT tree as it is grown self.PLOT_TREE_FAIL = False # Plot RRT tree in case of timeout # Settings self.connectDist = 1 # Euclidean distance between nodes for CONNECT self.closeEnoughDist = .25 self.closeEnoughAng = .5 self.fullMap = fullMap self.robot = robot if dipController is None: self.dipControl = DipolarController(k1=.5, k2=1.5, lambdaValue=3) else: self.dipControl = dipController self.plotter = plotter def updateMap(self, newMap): self.fullMap = newMap def setCloseEnoughVals(self, dist=None, ang=None): """ Set the values that are checked when closeEnoughDipole is called. """ if dist is not None: self.closeEnoughDist = dist if ang is not None: self.closeEnoughAng = ang def setConnectDist(self, dist): self.connectDist = dist def get2DPathLength(self, path): """ Get the length of a path along the set of 2D waypoints. :param path: A list of points or poses as numpy arrays. """ distances = [norm(path[i][:2] - path[i+1][:2]) for i in range(len(path)-1)] return sum(distances) def sampleDipole(self): """ Returns a random dipole within the map's C-Free """ randPoint = self.fullMap.cFree.sample(random) randAng = 2 * np.pi * random() return np.array([randPoint[0], randPoint[1], randAng]) def getDipoleDistanceHeur(self, dipoleStart, dipoleEnd): """ Calculate a heuristic distance from dipoleStart to dipoleEnd. The distance is a linear combination of the euclidean distance between dipoles, orientation difference between dipoles, orientation difference between dipoleStart and the field at dipoleStart from dipoleEnd, and the how far infront or behind dipoleStart is from the line orthogonal to to and passing through dipoleEnd. """ # Weights W_EUCL = 5 # Euclidian distance W_ORIENT = 1 # Difference between orientations W_FIELD = 1 # Difference between orientation and field W_FRONT_BEHIND = 0 # In front or behind dirV = dipoleStart[:2] - dipoleEnd[:2] euclD = norm(dirV) angleDiff = np.abs(diffAngles(dipoleEnd[2], dipoleStart[2])) dipoleField = self.dipControl.getFieldAt(dipoleStart, dipoleEnd) fieldAngle = np.arctan2(dipoleField[1], dipoleField[0]) fieldDiff = np.abs(diffAngles(fieldAngle, dipoleStart[2])) dipoleT = np.array(dipoleStart[:2])/norm(dipoleStart[:2]) relativePos = np.dot(dipoleT, dirV) totalDist = W_EUCL*euclD + W_ORIENT*angleDiff + \ W_FIELD*fieldDiff + W_FRONT_BEHIND*relativePos return totalDist def getNClosestIndicesDipole(self, dipole, tree, N=1): """ Uses the distance heuristic to return a list with the indices of the N closest nodes within the tree to dipole. """ distances = [self.getDipoleDistanceHeur(dipole, nodeX.getPose()) for nodeX in tree] closestIndices = sorted(range(len(distances)), key=lambda i: distances[i]) return closestIndices[:N] def getDipoleToDipolePath(self, dipoleStart, dipoleEnd, timeout = .5): """ Returns (connected, path) where connected is True if a collision free path was found, and path is the list of poses. :param timeout: The maximum time to spend in this function """ DT = .1 # The time interval for controls and forward integration poseCurr = np.array(dipoleStart) posePrev = poseCurr path = [poseCurr] timeStart = clock() while (clock() - timeStart) < timeout: u, w = self.dipControl.getControlls(poseCurr, dipoleEnd, posePrev, DT) posePrev = poseCurr poseCurr = self.dipControl.integrateForwards(posePrev, u, w, DT) path.append(poseCurr) # Check for collision self.robot.moveRobotTo(poseCurr) if not self.fullMap.cFree.covers(self.robot.shape): return (False, path) # If reached goal return the final state if self.closeEnoughDipole(poseCurr, dipoleEnd): return (True, path) # Timed out. This warning can be removed if bothersome. print "DEBUG: Warning getDipoleToDipolePath timed out." return (False, path) def closeEnoughDipole(self, currPose, endPose): """ Returns true if the euclidean and angular distances between currPose and endPose is below the set threshold. """ dist = norm(currPose[:2] - endPose[:2]) angDiff = np.abs(diffAngles(currPose[2], endPose[2])) if dist < self.closeEnoughDist and angDiff < self.closeEnoughAng: return True else: return False # TODO: Method has not been used in a long time. Must be updated. def extendTreeDipole(self, tree, dipole): """ Tries to extend the tree in the direction of the sampled dipole by one leaf. """ MAX_DIST = 1 # Choose the dipole to extend from closestNodeIndex = self.getNClosestIndicesDipole(dipole, tree, N=1)[0] closestNode = tree[closestNodeIndex] dirV = dipole[:2] - closestNode.getXY() dist = norm(dirV) if dist > MAX_DIST: dipole = np.array(dipole) dipole[:2] = closestNode.getXY() + (dirV/dist)*MAX_DIST pathFound, _ = self.getDipoleToDipolePath(closestNode.getPose(), dipole) if pathFound: tree.append(self.Node(pose=dipole, parent=closestNode)) return True else: return False def extendTreeDipoleConnect(self, tree, dipole): """ Finds the closest node in three to dipole and tries to extend from that node towards dipole in intervals of MAX_DIST for as many iterations as possible. Will consider the top NUM_CONSIDERED nodes to see if any will make progress and extend from one of them. New nodes are added to tree. """ MAX_DIST = self.connectDist NUM_CONSIDERED = 5 # Get the index of the closest nodes to dipole closestNodeIndex = self.getNClosestIndicesDipole(dipole, tree, N=NUM_CONSIDERED) # If no progress is made then try the next closest startTreeLen = len(tree) for i in closestNodeIndex: closestNode = tree[i] dirV = dipole[:2] - closestNode.getXY() dirAng = np.arctan2(dirV[1], dirV[0]) dist = norm(dirV) numIntervals = int(dist/MAX_DIST) dirV = dirV/dist*MAX_DIST # Normalize to step size # Try to connect to dipole in intervals collided = False parentT = closestNode dipoleCurr = np.array(parentT.getPose()) dipoleNext = np.array(dipoleCurr) dipoleNext[2] = dirAng for _ in range(numIntervals): dipoleNext[:2] += dirV[:2] connected, _ = self.getDipoleToDipolePath(dipoleCurr, dipoleNext) if not connected: collided = True break currNode = self.Node(pose=dipoleNext, parent=parentT) tree.append(currNode) parentT = currNode dipoleCurr = currNode.getPose() # Attempt to connect to the last node if not collided: connected, _ = self.getDipoleToDipolePath(dipoleCurr, dipole) if connected: currNode = self.Node(pose=dipole, parent=parentT) tree.append(currNode) # If the tree has grown then progress was made. Dont go to next neighbors if len(tree) > startTreeLen: return def getRRTDipoleControlPath(self, startPose, goalPoseList, goalBias=.2): """ Returns a path from startPose to an goalPoseList in the form of a list of Nodes. Uses a dipole controller to connect nodes in the RRT. """ goalBias = .2 # Percentage of attempt to connect to goalPoseList MAX_ITTER = 700 # Maximum number of tree iterations startPose = np.array(startPose).astype(float) goalPoseList = [np.array(goalPose).astype(float) for goalPose in goalPoseList] tree = [self.Node(pose=startPose, parent=None)] oldTreeLength = 1 for currIter in range(MAX_ITTER): # Select dipole to drive towards randNumber = random() if randNumber <= goalBias: randIndex = randint(0, len(goalPoseList)-1) randDipole = goalPoseList[randIndex] else: randDipole = self.sampleDipole() # Try to extend the tree # extended = extendTreeDipole(tree, randDipole, fullMap) self.extendTreeDipoleConnect(tree, randDipole) numNewNodes = len(tree) - oldTreeLength oldTreeLength = len(tree) if self.PLOT_TREE and self.plotter != None: # print "Tree: iteration: " + str(currIter) if numNewNodes > 0: if self.DEBUGER: self.plotter.drawMap(self.fullMap) self.plotter.drawTree(tree, color='k', width=1) plt.show() else: self.plotter.drawTree(tree[-numNewNodes:], color='k', width=2) for goalPose in goalPoseList: self.plotter.drawStartAndGoalPoints(startPose, goalPose) # Reached end finished = False for goalPose in goalPoseList: if self.closeEnoughDipole(tree[-1].getPose(), goalPose): finished = True break if finished: print "Tree total iterations: " + str(currIter) path = [] currNode = tree[-1] while currNode != None: path.append(currNode) currNode = currNode.parent path.reverse() return path if self.PLOT_TREE_FAIL: plt.ioff() self.plotter.drawMap(self.fullMap) self.plotter.drawTree(tree, color='k', width=1) plt.show() return None def getShortcutPathDipole(self, path, additionalGoals=None): """ Use the shortcut heuristic to return a shorter path in the form of a list of Nodes. The final node could be the last node in path or one of the nodes from additionalGoals Implements Dijkstras to find new connections in the given path. :param path: A list of Nodes :param additionalGoals: A list of Nodes """ numEdges = 0 # Keep track of number of edges calculated # Get a list of unique goals if additionalGoals is not None: allGoalNodes = [node for node in additionalGoals] else: allGoalNodes = [] duplicate = False nodeT = path[-1] for node in allGoalNodes: if norm(nodeT.getPose() - node.getPose()) == 0: duplicate = True break if not duplicate: allGoalNodes.append(nodeT) allNodes = [node for node in path[:-1]] + allGoalNodes # Setup numNodesTotal = len(allNodes) nodeIQueue = PriorityQueue() # Node to expand (distance, nodeIndex) visited = [False]*numNodesTotal distances = {} # (nodeIndex:distanceFromStart) # Initialize nodeIQueue.put((0,0)) distances[0] = 0 path[0].parent = None while not nodeIQueue.empty(): currNodeI = nodeIQueue.get()[1] if visited[currNodeI]: continue currDist = distances[currNodeI] currNode = allNodes[currNodeI] visited[currNodeI] = True # Check if reached Goal if currNode in allGoalNodes: print "ShortcutDipole numEdges: " + str(numEdges) shortPath = [] while currNode != None: shortPath.append(currNode) currNode = currNode.parent shortPath.reverse() return shortPath # Go through all the non visited nodes for neighborI in range(numNodesTotal): if visited[neighborI]: continue neighNode = allNodes[neighborI] numEdges += 1 # Calculate alternative distance pathFound, pathDi = self.getDipoleToDipolePath(currNode.getPose(), neighNode.getPose()) if not pathFound: continue pathDi += [neighNode.getPose()] distAlternative = currDist + self.get2DPathLength(pathDi) # If shorter than recorded update if distAlternative < distances.get(neighborI, float('inf')): nodeIQueue.put((distAlternative, neighborI)) distances[neighborI] = distAlternative neighNode.parent = currNode # No path was found return None def getThetaStarPath(self, path, additionalGoals=None): """ Use the ThetaStar to return a shorter path in the form of a list of Nodes. The final node could be the last node in path or one of the nodes from additionalGoals. :param path: A list of Nodes :param additionalGoals: A list of Nodes """ # Get a list of unique goals if additionalGoals is not None: allGoalNodes = [node for node in additionalGoals] else: allGoalNodes = [] duplicate = False nodeT = path[-1] for node in allGoalNodes: if norm(nodeT.getPose() - node.getPose()) == 0: duplicate = True break if not duplicate: allGoalNodes.append(nodeT) # The path without counting the goal partialPath = [node for node in path[:-1]] newPath = partialPath[:2] _, pathDipoleT = self.getDipoleToDipolePath(newPath[0].getPose(), newPath[1].getPose()) pathDipoleT.append(newPath[1].getPose()) lengthPrev = self.get2DPathLength(pathDipoleT) # Run theta star on the partialPath for node in partialPath[2:]: # Calculate distance to new node through previous _, pathDipoleT = self.getDipoleToDipolePath(newPath[-1].getPose(), node.getPose()) length1 = self.get2DPathLength(pathDipoleT + [node.getPose()]) # Check if find a shorter path through the prevous node's parent pathFound2, pathDipoleT = self.getDipoleToDipolePath( newPath[-2].getPose(), node.getPose()) # Could not connect to the previous node's parent if not pathFound2: newPath.append(node) lengthPrev = length1 continue length2 = self.get2DPathLength(pathDipoleT + [node.getPose()]) # First path is shorter if (lengthPrev + length1) < length2: newPath.append(node) lengthPrev = length1 continue # Second path is shorter newPath[-1] = node lengthPrev = length2 # Run theta star for each of the goals endDistancesAndNodes = [] for goal in allGoalNodes: # Calculate distance to goal node through previous pathFound1, pathDipoleT = self.getDipoleToDipolePath(newPath[-1].getPose(), goal.getPose()) length1 = self.get2DPathLength(pathDipoleT + [goal.getPose()]) # Check if find a shorter path through the prevous node's parent pathFound2, pathDipoleT = self.getDipoleToDipolePath( newPath[-2].getPose(), goal.getPose()) length2 = self.get2DPathLength(pathDipoleT + [goal.getPose()]) if pathFound1 and not pathFound2: endNodesT = [newPath[-1], goal] endDistancesAndNodes.append((lengthPrev + length1, endNodesT)) elif not pathFound1 and pathFound2: endDistancesAndNodes.append((length2, [goal])) elif pathFound1 and pathFound2: # First path is shorter if (lengthPrev + length1) < length2: endNodesT = [newPath[-1], goal] endDistancesAndNodes.append((lengthPrev + length1, endNodesT)) # Second path is shorter else: endDistancesAndNodes.append((length2, [goal])) closestGoalI = min(range(len(endDistancesAndNodes)), key=lambda x: endDistancesAndNodes[x][0]) return newPath[:-1] + endDistancesAndNodes[closestGoalI][1] def get2DPathRepresent(self, path): """ Takes a path as a list of nodes and runs the dipolar controler to return a close approximation of the path traveled as a list of dipoles """ pathTraveled = [path[0].getPose()] for i in range(len(path)-1): _, pathDi = self.getDipoleToDipolePath(path[i].getPose(), path[i+1].getPose()) pathDi.append(path[i+1].getPose()) pathTraveled += pathDi return pathTraveled def nodesToDipoles(self, path): return [node.getPose() for node in path] def dipolesToNodes(self, dipoleList): """ Takes in a list of dipoles and outputs a list of Nodes """ return [self.Node(pose=dipole) for dipole in dipoleList] class Node: def __init__(self, x=0, y=0, theta=0, XY=None, pose=None, parent=None): """ An object to represent poses and connections amongst them. If instantiated, the value of later parameters will override the previous (ex. pose can override all) :param x: The x coordinate :param y: The y coordinate :param theta: The orientation in radians :param XY: List with x and y :param pose: List with x, y, and theta :param parent: The node from which it stems """ self.x = x self.y = y self.theta = theta if XY != None: self.x = XY[0] self.y = XY[1] if pose != None: self.x = pose[0] self.y = pose[1] self.theta = pose[2] self.parent = parent def getXY(self): return np.array([self.x, self.y]) def getPose(self): return np.array([self.x, self.y, self.theta])