Exemplo n.º 1
0
 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
Exemplo n.º 2
0
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)