def __init__(self, proj, shared_data, robotType, nodeDistInter, linearGain, angularGain, plotTree, plotPath, plotRegion): """ An RRT with dipoles for connecting nodes. robotType (int): The robot shape to be used. Default is circle with radius .1 (default=0) nodeDistInter (float): The max euclidean distance between nodes. (default=1) linearGain (float): The k1 gain for the dipolar closed loop controller. (default=0.5) angularGain (float): the k2 gain for the dipolar closed loop controller. (default=1.5) plotTree (bool): Plot the RRT Live plotPath (bool): Plot paths after calculation plotRegion (bool): Plot the current and next region """ # Settings self.DEBUG = False # Print statements for debugging self.DEBUGER = False # If using a debugger. Matplotlib workaround self.PLOT_REG = plotRegion # Plot the current and next region self.PLOT_TREE = plotTree # Plot the RRT live self.PLOT_PATH = plotPath # Plot path generated self.PLOT_TREE_FAIL = True # Plot the RRT if it fails to find a path # Get references to handlers we'll need to communicate with self.drive_handler = proj.h_instance['drive'] self.pose_handler = proj.h_instance['pose'] # Get information about regions self.rfi = proj.rfi self.coordmap_map2lab = proj.coordmap_map2lab # Plotter self.plotter = RRTPlotter(invertY=False) self.plotter.ion() # Turn on interactive mode # Dipolar controller self.dipController = DipolarController(k1=linearGain) self.prevPose = self.pose_handler.getPose() self.dT = 1/20.0 # The time elapsed since the call to controller # RRT Variables self.robot = self.getRobot(robotType) self.closeEnoughDist = self.robot.radius # The max distance from waypoint self.closeEnoughAng = .3 # The max angle difference from pose self.nodeDistInter = nodeDistInter self.path = None self.nextWaypointIndex = None self.storedNextReg = None self.rrt = DipolarRRT(None, self.robot, plotter=self.plotter, dipController=self.dipController) self.rrt.setCloseEnoughVals(self.closeEnoughDist, self.closeEnoughAng) self.rrt.setConnectDist(nodeDistInter) self.rrt.DEBUGER = self.DEBUGER self.rrt.PLOT_TREE = self.PLOT_TREE self.rrt.PLOT_TREE_FAIL = self.PLOT_TREE_FAIL self.rrt.connectDist = self.nodeDistInter
class motionControlHandler: def __init__(self, proj, shared_data, robotType, nodeDistInter, linearGain, angularGain, plotTree, plotPath, plotRegion): """ An RRT with dipoles for connecting nodes. robotType (int): The robot shape to be used. Default is circle with radius .1 (default=0) nodeDistInter (float): The max euclidean distance between nodes. (default=1) linearGain (float): The k1 gain for the dipolar closed loop controller. (default=0.5) angularGain (float): the k2 gain for the dipolar closed loop controller. (default=1.5) plotTree (bool): Plot the RRT Live plotPath (bool): Plot paths after calculation plotRegion (bool): Plot the current and next region """ # Settings self.DEBUG = False # Print statements for debugging self.DEBUGER = False # If using a debugger. Matplotlib workaround self.PLOT_REG = plotRegion # Plot the current and next region self.PLOT_TREE = plotTree # Plot the RRT live self.PLOT_PATH = plotPath # Plot path generated self.PLOT_TREE_FAIL = True # Plot the RRT if it fails to find a path # Get references to handlers we'll need to communicate with self.drive_handler = proj.h_instance['drive'] self.pose_handler = proj.h_instance['pose'] # Get information about regions self.rfi = proj.rfi self.coordmap_map2lab = proj.coordmap_map2lab # Plotter self.plotter = RRTPlotter(invertY=False) self.plotter.ion() # Turn on interactive mode # Dipolar controller self.dipController = DipolarController(k1=linearGain) self.prevPose = self.pose_handler.getPose() self.dT = 1/20.0 # The time elapsed since the call to controller # RRT Variables self.robot = self.getRobot(robotType) self.closeEnoughDist = self.robot.radius # The max distance from waypoint self.closeEnoughAng = .3 # The max angle difference from pose self.nodeDistInter = nodeDistInter self.path = None self.nextWaypointIndex = None self.storedNextReg = None self.rrt = DipolarRRT(None, self.robot, plotter=self.plotter, dipController=self.dipController) self.rrt.setCloseEnoughVals(self.closeEnoughDist, self.closeEnoughAng) self.rrt.setConnectDist(nodeDistInter) self.rrt.DEBUGER = self.DEBUGER self.rrt.PLOT_TREE = self.PLOT_TREE self.rrt.PLOT_TREE_FAIL = self.PLOT_TREE_FAIL self.rrt.connectDist = self.nodeDistInter def gotoRegion(self, current_reg, next_reg, last=False): """ Returns ``True`` if we've reached the next region. Uses the generated path and dipoles to go to the next waypoint. If the desired region changes or no path currently excists, it will create one using the RRT """ if current_reg == next_reg and not last: # No need to move! self.drive_handler.setVelocity(0, 0) # So let's stop return True # Already there # Check if a new path needs to be calculated if self.path is None or self.storedNextReg != next_reg: self.drive_handler.setVelocity(0, 0) self.findAndSetPath(current_reg, next_reg) self.storedNextReg = next_reg # Find our current configuration pose = self.pose_handler.getPose() # Check if reached waypoint while self.closeEnough(pose, self.path[self.nextWaypointIndex]): self.nextWaypointIndex += 1 # Check end of path if self.nextWaypointIndex >= len(self.path): self.path = None self.drive_handler.setVelocity(0, 0) return True # Already there # Calculate the linear and angular velocities nextWaypoint = self.path[self.nextWaypointIndex] v, w = self.dipController.getControlls(pose, nextWaypoint, self.prevPose, self.dT) self.drive_handler.setVelocity(v, w) self.prevPose = pose # if self.DEBUG: # print "-----------------" # print "Current pose:", pose # print "Next waypoint:", nextWaypoint # print "V:", v, " W:", w return False def findAndSetPath(self, current_reg, next_reg): """ Calculate the path to go from current_reg to next_reg. """ # Current and next regions currRegBoundary, currRegObstList = self.getRegionPolygons(current_reg) nextRegBoundary, nextRegObstList = self.getRegionPolygons(next_reg) allObstacles = currRegObstList + nextRegObstList currRegPoly = currRegBoundary for obst in currRegObstList: currRegPoly -= obst nextRegPoly = nextRegBoundary for obst in nextRegObstList: nextRegPoly -= obst # Prepare RRT rrtMap = RRTMap(currRegPoly + nextRegPoly) pose = self.pose_handler.getPose() self.rrt.updateMap(rrtMap) # Goal poses goalPoseList = self.getGoalPoses(current_reg, next_reg, nextRegPoly) if len(goalPoseList) == 0: raise Exception("Error: DipolarRRTController - No goal pose found.") if self.DEBUG: print "GoalPose:", goalPoseList # Start plotting and generating the path self.plotter.clearPlot() if self.PLOT_REG: # self.plotter.drawMap(rrtMap) self.plotter.drawPolygon(currRegBoundary, color='r', width=3) self.plotter.drawPolygon(nextRegBoundary, color='g', width=3) for obst in allObstacles: self.plotter.drawPolygon(obst, color='k', width=3) for goalPose in goalPoseList: self.plotter.drawStartAndGoalRobotShape(pose, goalPose, self.robot) if self.DEBUG: print "Getting RRTPath" rrtPath = self.rrt.getRRTDipoleControlPath(pose, goalPoseList) if self.PLOT_PATH: pathT = self.rrt.get2DPathRepresent(rrtPath) self.plotter.drawDipolePath2D(pathT, color='g', width=3) if self.DEBUG: print "Getting Short Path" # if self.DEBUG: # # x = 5. # # y = 1 # # robotCopy = self.robot.copy() # # robotCopy.moveRobotTo([x, y, 0]) # # self.plotter.drawPolygon(robotCopy.shape) # # self.plotter.drawPolygon(rrtMap.cFree, color='b', width=3) # # self.plotter.drawPolygon(rrtMap.boundary, color='k', width=3) # # print rrtMap.cFree.covers(robotCopy.shape) # self.plotter.drawPolygon(currRegPoly, color='g', width=3) # print "Current", currRegPoly # self.plotter.drawPolygon(nextRegPoly, color='g', width=3) # print "Next", nextRegPoly # time.sleep(2) # sumT = currRegPoly + nextRegPoly # self.plotter.drawPolygon(sumT, color='r', width=3) # print "Sum", sumT # time.sleep(2) # self.plotter.drawPolygon(rrtMap.boundary, color='k', width=3) # time.sleep(2) # Polygon allGoalNodes = self.rrt.dipolesToNodes(goalPoseList) shortPath = self.rrt.getShortcutPathDipole(rrtPath, additionalGoals=allGoalNodes) # shortPath = self.rrt.getThetaStarPath(rrtPath, additionalGoals=allGoalNodes) if self.PLOT_PATH: pathT = self.rrt.get2DPathRepresent(shortPath) self.plotter.drawDipolePath2D(pathT, color='r', width=3) if self.DEBUG: pathT = self.rrt.get2DPathRepresent(shortPath) print "Length of pathT=", len(pathT) self.plotter.drawRobotOccupiedPath(pathT, self.robot, color='b', width=1) # Update instance fields self.path = self.rrt.nodesToDipoles(shortPath) # Convert to 3D vector self.nextWaypointIndex = 0 self.storedNextReg = next_reg def getRegionPolygons(self, targetReg): """ Returns a polygon representing the region boundary and a list of polygons representing the obstacles or holes in the region. """ # Get boundary polygon region = self.rfi.regions[targetReg] boundaryPoints = [x for x in region.getPoints()] boundaryPoints = map(self.coordmap_map2lab, boundaryPoints) regionPoly = Polygon(boundaryPoints) # Remove holes obstacleList = [] for holeId in range(len(region.holeList)): pointsT = [x for x in region.getPoints(hole_id=holeId)] pointsT = map(self.coordmap_map2lab, pointsT) obstacleList.append(Polygon(pointsT)) return regionPoly, obstacleList def getGoalPoses(self, currRegion, nextRegion, nextRegPoly): """ Returns a list of numpy 1 x 3 numpy arrays with possible goal poses For each transition face between the two regions: Find the center point and place goal points such that they extend from the center points in the direction of the normal to the transition face. The normal should point into the next region and the points will be a distance slightly larger than robot.backLen away from the center point from which they extend. Goal poses will be the goal points with the direction of the face's normal as the third element. Take a deep breath and read that over. """ # distIntoPoly = self.robot.backLen * 1.1 + self.closeEnoughDist + 20 distIntoPoly = self.robot.radius * 1.1 + self.closeEnoughDist robotCopy = self.robot.copy() goalPoses = [] for transFace in self.rfi.transitions[currRegion][nextRegion]: transFacePoints = [x for x in transFace] transFacePoints = np.array(map(self.coordmap_map2lab, transFacePoints)) center = (transFacePoints[0,:] + transFacePoints[1,:]) / 2 faceDir = transFacePoints[1,:] - transFacePoints[0,:] faceNorm = np.array([faceDir[1], -faceDir[0]]) faceNorm = faceNorm/np.linalg.norm(faceNorm) * distIntoPoly # Check for correct direction face norm pointT = center + faceNorm if not nextRegPoly.isInside(pointT[0], pointT[1]): faceNorm *= -1 # Direction and point for the goalPose dirT = np.arctan2(faceNorm[1], faceNorm[0]) pointT = center + faceNorm poseT = np.array([pointT[0], pointT[1], dirT]) robotCopy.moveRobotTo(poseT) if nextRegPoly.covers(robotCopy.shape): goalPoses.append(poseT) # print "TPoints:", transFacePoints # print "C:", center # print "Norm:", faceNorm # print "PoseT:", poseT return goalPoses def closeEnough(self, pose1, pose2): """ Returns true if pose1 and pose2 are within a threshold distance and angle difference. """ dist = np.linalg.norm(pose1[:2] - pose2[:2]) angDiff = abs(diffAngles(pose1[2], pose2[2])) if dist < self.closeEnoughDist and angDiff < self.closeEnoughAng: return True else: return False def getRobot(self, robotType): """ Returns a predefined RRTRobot """ # Simulated Circular robot (For when lab to map matrix is the identity) if robotType == 0: return RRTRobot.circularRobot([0,0,0], 10) # Create elif robotType == 1: return RRTRobot.circularRobot([0,0,0], .1) else: msg = "ERROR: DipolarRRTController - Undefined robot type." raise Exception(msg)