Esempio n. 1
0
    def generateNewNode(self,V,V_theta,E,Other,regionPoly,stuck,append_after_latest_node =False):
        """
        Generate a new node on the current tree matrix
        V         : the node matrix
        V_theta   : the orientation matrix
        E         : the tree matrix (or edge matrix)
        Other     : the matrix containing the velocity and angular velocity(omega) information
        regionPoly: the polygon of current region
        stuck     : count on the number of times failed to generate new node
        append_after_latest_node : append new nodes to the latest node (True only if the previous node addition is successful)
        """


        if self.system_print == True:
            print "In control space generating path,stuck = " + str(stuck)

        connection_to_tree = False   # True when connection to the tree is successful

        if stuck > self.stuck_thres:
            # increase the range of omega since path cannot ge generated
            omega = random.choice(self.omega_range_escape)
        else:
            #!!!! CONTROL SPACE STEP 1 - generate random omega
            omega = random.choice(self.omega_range)


        #!!!! CONTROL SPACE STEP 2 - pick a random point on the tree
        if append_after_latest_node:
            tree_index = shape(V)[1]-1
        else:
            if random.choice([1,2]) == 1:
                tree_index = random.choice(array(V[0])[0])
            else:
                tree_index = shape(V)[1]-1


        xPrev     = V[1,tree_index]
        yPrev     = V[2,tree_index]
        thetaPrev = V_theta[tree_index]

        j = 1
        #!!!! CONTROL SPACE STEP 3 - Check path of the robot
        path_robot = PolyShapes.Circle(self.radius,(xPrev,yPrev))
        while j <= self.timeStep:
            xOrg      = xPrev
            yOrg      = yPrev
            xPrev     = xPrev + self.velocity/omega*(sin(omega* 1 + thetaPrev)-sin(thetaPrev))
            yPrev     = yPrev - self.velocity/omega*(cos(omega* 1 + thetaPrev)-cos(thetaPrev))
            thetaPrev = omega* 1 + thetaPrev
            path_robot = path_robot + PolyShapes.Circle(self.radius,(xPrev,yPrev))

            j = j + 1

        thetaPrev = self.orientation_bound(thetaPrev)
        path_all = PolyUtils.convexHull(path_robot)
        in_bound = regionPoly.covers(path_all)
        """
        # plotting
        if plotting == True:
            self.plotPoly(path_all,'r',1)
        """

        stuck = stuck + 1

        if in_bound:
            robot_new_node = PolyShapes.Circle(self.radius,(xPrev,yPrev))
            # check how many nodes on the tree does the new node overlaps with
            nodes_overlap_count = 0
            for k in range(shape(V)[1]-1):
                robot_old_node = PolyShapes.Circle(self.radius,(V[1,k],V[2,k]))
                if robot_new_node.overlaps(robot_old_node):
                    if  abs(thetaPrev - V_theta[k]) <   self.max_angle_overlap:
                        nodes_overlap_count += 1


            if nodes_overlap_count == 0 or (stuck > self.stuck_thres+1 and nodes_overlap_count < 2) or (stuck > self.stuck_thres+500):
                if  stuck > self.stuck_thres+1:
                    append_after_latest_node  = False

                if (stuck > self.stuck_thres+500):
                    stuck = 0
                stuck = stuck - 20
                # plotting
                if self.plotting == True:
                    self.plotPoly(path_all,'b',1)

                if self.system_print == True:
                    print "node connected"

                V = hstack((V,vstack((shape(V)[1],xPrev,yPrev))))
                V_theta = hstack((V_theta,thetaPrev))
                E = hstack((E,vstack((tree_index ,shape(V)[1]-1))))
                Other = hstack((Other,vstack((self.velocity,omega))))
                ##################### E should add omega and velocity
                connection_to_tree = True
                append_after_latest_node  = True
            else:
                append_after_latest_node = False

                if self.system_print == True:
                    print "node not connected. check goal point"

        else:
            append_after_latest_node = False

        return  V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree
Esempio n. 2
0
    def gotoRegion(self, current_reg, next_reg, last=False):
        """
        If ``last`` is True, we will move to the center of the destination region.
        Returns ``True`` if we've reached the destination region.
        """
        q_gBundle = [[], []]  # goal bundle
        q_overlap = [[], []]  # overlapping points with robot range
        pose = self.pose_handler.getPose()  # Find our current configuration

        #Plot the robot on the map in figure 1
        if self.PLOT == True:
            plt.figure(self.original_figure)
            plt.clf()
            self.plotPoly(self.realRobot, 'r')
            self.plotPoly(self.robot, 'b')
            plt.plot(pose[0], pose[1], 'bo')
            self.plotPioneer(self.original_figure)

        if current_reg == next_reg and not last:
            # No need to move!
            self.drive_handler.setVelocity(0, 0)  # So let's stop
            return False

        # Check if Vicon has cut out
        if math.isnan(pose[2]):
            print "no vicon pose"
            print "WARNING: No Vicon data! Pausing."
            #self.drive_handler.setVelocity(0, 0)  # So let's stop
            time.sleep(1)
            return False

        ###This part is run when the robot goes to a new region, otherwise, the original map will be used.
        if not self.previous_current_reg == current_reg:
            #print 'getting into bug alogorithm'

            #clean up the previous self.map_work
            self.map_work = Polygon.Polygon()

            # NOTE: Information about region geometry can be found in self.proj.rfi.regions
            # create polygon list for regions other than the current_reg and the next_reg
            self.map_work += self.map[self.proj.rfi.regions[current_reg].name]
            self.map_work += self.map[self.proj.rfi.regions[next_reg].name]

            # building current polygon and destination polygon
            self.nextRegionPoly = self.map[
                self.proj.rfi.regions[next_reg].name]
            self.currentRegionPoly = self.map[
                self.proj.rfi.regions[current_reg].name]

            #set to zero velocity before finding the tranFace
            self.drive_handler.setVelocity(0, 0)
            if last:
                transFace = None
            else:
                print "Current reg is " + str(
                    self.proj.rfi.regions[current_reg].name.lower())
                print "Next reg is " + str(
                    self.proj.rfi.regions[next_reg].name.lower())

                for i in range(
                        len(self.proj.rfi.transitions[current_reg][next_reg])):
                    pointArray_transface = [
                        x for x in self.proj.rfi.transitions[current_reg]
                        [next_reg][i]
                    ]
                    transFace = asarray(
                        map(self.coordmap_map2lab, pointArray_transface))
                    bundle_x = (transFace[0, 0] +
                                transFace[1, 0]) / 2  #mid-point coordinate x
                    bundle_y = (transFace[0, 1] +
                                transFace[1, 1]) / 2  #mid-point coordinate y
                    q_gBundle = hstack((q_gBundle, vstack(
                        (bundle_x, bundle_y))))
                q_gBundle = q_gBundle.transpose()

                # Find the closest face to the current position
                max_magsq = 1000000
                for tf in q_gBundle:
                    magsq = (tf[0] - pose[0])**2 + (tf[1] - pose[1])**2
                    if magsq < max_magsq:
                        connection = 0
                        tf = tf + (tf - asarray(self.currentRegionPoly.center(
                        ))) / norm(tf - asarray(self.currentRegionPoly.center(
                        ))) * 2.1 * self.PioneerLengthHalf
                        if not self.nextRegionPoly.covers(
                                PolyShapes.Circle(self.PioneerLengthHalf * 2,
                                                  (tf[0], tf[1]))):
                            tf = tf - (tf - asarray(
                                self.currentRegionPoly.center())) / norm(
                                    tf - asarray(self.currentRegionPoly.center(
                                    ))) * 4.2 * self.PioneerLengthHalf
                            if self.nextRegionPoly.covers(
                                    PolyShapes.Circle(
                                        self.PioneerLengthHalf * 2,
                                        (tf[0], tf[1]))):
                                connection = 1
                        else:
                            connection = 1
                        if connection == 1:
                            pt1 = tf
                            max_magsq = magsq
                            transFace = 1
                            self.q_g[0] = pt1[0]
                            self.q_g[1] = pt1[1]
                        else:
                            sample = False
                            while not sample:
                                self.q_g[0], self.q_g[
                                    1] = self.nextRegionPoly.sample(
                                        random.random)
                                robo = PolyShapes.Circle(
                                    self.PioneerLengthHalf,
                                    (self.q_g[0], self.q_g[1]))
                                if not bool(robo - self.nextRegionPoly):
                                    sample = True
                """
                # Push the goal point to somewhere inside the next region to ensure the robot will get there.(CHECK!!)
                self.q_g = self.q_g+(self.q_g-asarray(self.currentRegionPoly.center()))/norm(self.q_g-asarray(self.currentRegionPoly.center()))*3*self.PioneerLengthHalf
                if not self.nextRegionPoly.isInside(self.q_g[0],self.q_g[1]):
                    self.q_g = self.q_g-(self.q_g-asarray(self.currentRegionPoly.center()))/norm(self.q_g-asarray(self.currentRegionPoly.center()))*6*self.PioneerLengthHalf
                """

                #plot exiting point
                if self.PLOT_EXIT == True:
                    plt.figure(self.overlap_figure)
                    plt.clf()
                    plt.plot(q_gBundle[:, 0], q_gBundle[:, 1], 'ko')
                    plt.plot(self.q_g[0], self.q_g[1], 'ro')
                    plt.plot(pose[0], pose[1], 'bo')
                    self.plotPioneer(self.overlap_figure, 0)

                if transFace is None:
                    print "ERROR: Unable to find transition face between regions %s and %s.  Please check the decomposition (try viewing projectname_decomposed.regions in RegionEditor or a text editor)." % (
                        self.proj.rfi.regions[current_reg].name,
                        self.proj.rfi.regions[next_reg].name)

        ##################################################
        #######check whether obstacle is detected#########
        ##################################################

        #Update pose,update self.robot, self.realRobot orientation
        self.robot.shift(pose[0] - self.prev_pose[0],
                         pose[1] - self.prev_pose[1])
        self.realRobot.shift(pose[0] - self.prev_pose[0],
                             pose[1] - self.prev_pose[1])
        self.robot.rotate(pose[2] - self.prev_pose[2], pose[0], pose[1])
        self.realRobot.rotate(pose[2] - self.prev_pose[2], pose[0], pose[1])
        self.prev_pose = pose

        ############################
        ########### STEP 1##########
        ############################
        ##Check whether obsRange overlaps with obstacle or the boundary (overlap returns the part of robot not covered by the region

        # for real Pioneer robot
        if self.system == 1:
            # motion controller is not in boundary following mode
            if self.boundary_following == False:

                if self.robocomm.getReceiveObs() == False:
                    overlap = self.robot - (self.map_work)
                else:
                    overlap = self.robot - (self.map_work -
                                            self.robocomm.getObsPoly())

            else:  #TRUE
                # use a robot with full range all around it
                Robot = PolyShapes.Circle(self.obsRange, (pose[0], pose[1]))
                Robot.shift(self.shift * cos(pose[2]),
                            self.shift * sin(pose[2]))

                if self.robocomm.getReceiveObs() == False:
                    overlap = Robot - (self.map_work)
                else:
                    overlap = Robot - (self.map_work -
                                       self.robocomm.getObsPoly())
        # for ODE
        else:
            if self.boundary_following == False:
                overlap = self.robot - (self.map_work)
            else:  #TRUE
                overlap = self.robot - (self.map_work)

        if self.boundary_following == False:
            if bool(overlap):  ## overlap of obstacles
                #print "There MAYBE overlap~~ check connection to goal"

                # check whether the real robot or and path to goal overlap with the obstacle
                QGoalPoly = PolyShapes.Circle(self.PioneerLengthHalf,
                                              (self.q_g[0], self.q_g[1]))
                path = PolyUtils.convexHull(self.realRobot + QGoalPoly)
                if self.system == 1:
                    if self.robocomm.getReceiveObs() == False:
                        pathOverlap = path - (self.map_work)
                    else:
                        pathOverlap = path - (self.map_work -
                                              self.robocomm.getObsPoly())
                else:
                    pathOverlap = path - (self.map_work)

                if bool(
                        pathOverlap
                ):  # there is overlapping, go into bounding following mode
                    #print "There IS overlap"
                    self.q_hit = mat([pose[0], pose[1]]).T
                    self.boundary_following = True

                    #Generate m-line polygon
                    QHitPoly = PolyShapes.Circle(self.PioneerLengthHalf / 4,
                                                 (pose[0], pose[1]))
                    QGoalPoly = PolyShapes.Circle(self.PioneerLengthHalf / 4,
                                                  (self.q_g[0], self.q_g[1]))
                    self.m_line = PolyUtils.convexHull(QHitPoly + QGoalPoly)

                    #plot the first overlap
                    if self.PLOT_M_LINE == True:
                        plt.figure(self.overlap_figure)
                        plt.clf()
                        self.plotPoly(QHitPoly, 'k')
                        self.plotPoly(QGoalPoly, 'k')
                        self.plotPoly(overlap, 'g')
                        self.plotPoly(self.m_line, 'b')
                        plt.plot(pose[0], pose[1], 'bo')
                        self.plotPioneer(self.overlap_figure, 0)

                else:  ##head towards the q_goal
                    if self.system == 1:
                        if self.robocomm.getReceiveObs(
                        ) == False:  # wait for obstacles from Pioneer
                            vx = 0
                            vy = 0
                        else:
                            dis_cur = vstack((self.q_g[0], self.q_g[1])) - mat(
                                [pose[0], pose[1]]).T
                            vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
                            vy = (dis_cur / norm(dis_cur) / 3)[1, 0]

                    else:
                        dis_cur = vstack((self.q_g[0], self.q_g[1])) - mat(
                            [pose[0], pose[1]]).T
                        vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
                        vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
                        #print "no obstacles 2-ODE true"

            else:  ##head towards the q_goal
                if self.system == 1:
                    if self.robocomm.getReceiveObs(
                    ) == False:  # wait for obstacles from Pioneer
                        vx = 0
                        vy = 0
                    else:
                        dis_cur = vstack((self.q_g[0], self.q_g[1])) - mat(
                            [pose[0], pose[1]]).T
                        vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
                        vy = (dis_cur / norm(dis_cur) / 3)[1, 0]

                else:
                    dis_cur = vstack(
                        (self.q_g[0], self.q_g[1])) - mat([pose[0], pose[1]]).T
                    vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
                    vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
                    #print "no obstacles 1-ODE true"

        if self.boundary_following == True:
            self.q_hit_count += 1
            # finding the point to go normal to (closest overlapping point)

            j = 0
            recheck = 0
            while not bool(overlap):
                # cannot see the obstacle. Based on the location of the previous point blow up the range of the robot on the left or on the right
                j += 1

                # finding whether the previous obstacle point is on the left side or the right side of the robot
                # angle = angle of the previous point from the x-axis of the field
                # omega = angle of the current Pioneer orientation from the x-axis of the field
                # cc    = differnece between angle and omega ( < pi = previous point on the left of robot, else on the right of robot)
                x = self.prev_follow[0] - pose[0]
                y = self.prev_follow[1] - pose[1]
                angle = atan(y / x)

                # convert angle to 2pi
                if x > 0 and y > 0:
                    angle = angle
                elif x < 0 and y > 0:
                    angle = pi + angle
                elif x < 0 and y < 0:
                    angle = pi + angle
                else:
                    angle = 2 * pi + angle

                # convert pose to 2pi
                if pose[2] < 0:
                    omega = (2 * pi + pose[2])
                else:
                    omega = pose[2]

                if omega > angle:
                    cc = 2 * pi - (omega - angle)
                else:
                    cc = angle - omega

                # on the left
                #if angle - omega > 0 and angle - omega < pi:
                if cc < pi:
                    #print "on the left, angle: "+ str(angle) + " omega: "+ str(omega)+ " angle-omega: "+ str(angle-omega)
                    Robot = PolyShapes.Rectangle(self.range * 2 * j,
                                                 self.range * 2 * j)
                    Robot.shift(pose[0] - self.range * j * 2,
                                pose[1] - self.range * j)
                    Robot.rotate(pose[2] - pi / 2, pose[0], pose[1])

                # on the right
                else:
                    #print "on the right, angle: "+ str(angle) + " omega: "+ str(omega)+ " angle-omega: "+ str(angle-omega)
                    Robot = PolyShapes.Rectangle(self.range * 2 * j,
                                                 self.range * 2 * j)
                    Robot.shift(pose[0], pose[1] - self.range * j)
                    Robot.rotate(pose[2] - pi / 2, pose[0], pose[1])

                if self.system == 1:
                    overlap = Robot - (self.map_work -
                                       self.robocomm.getObsPoly())
                else:
                    overlap = Robot - (self.map_work)

                #self.plotPoly(Robot, 'm',2)
                #determines as dynamic obstacles and can be go striaight to the goal point
                if j >= 2:
                    dis_cur = vstack(
                        (self.q_g[0], self.q_g[1])) - mat([pose[0], pose[1]]).T
                    vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
                    vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
                    overlap = None
                    self.overlap = overlap
                    self.q_hit_count = 0
                    self.boundary_following = False
                    self.m_line = None
                    self.drive_handler.setVelocity(vx, vy, pose[2])
                    RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf +
                                                  0.06,
                                                  (pose[0], pose[1]))  ###0.05
                    departed = not self.currentRegionPoly.overlaps(
                        self.realRobot)
                    #departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
                    arrived = self.nextRegionPoly.covers(self.realRobot)
                    return arrived

            ##extra box plotting in figure 1#
            if self.PLOT_OVERLAP == True:
                plt.figure(self.original_figure)
                plt.clf()
                self.plotPoly(self.realRobot, 'r')
                self.plotPoly(self.robot, 'b')
                self.plotPoly(overlap, 'g', 3)
                plt.plot(pose[0], pose[1], 'bo')
                self.plotPioneer(self.original_figure)

            # find the closest point on the obstacle to the robot
            overlap_len = len(overlap)
            for j in range(overlap_len):
                BoundPolyPoints = asarray(overlap[j])
                for i in range(len(BoundPolyPoints) - 1):
                    bundle_x = (
                        BoundPolyPoints[i, 0] +
                        BoundPolyPoints[1 + i, 0]) / 2  #mid-point coordinate x
                    bundle_y = (
                        BoundPolyPoints[i, 1] +
                        BoundPolyPoints[1 + i, 1]) / 2  #mid-point coordinate y
                    q_overlap = hstack((q_overlap, vstack(
                        (bundle_x, bundle_y))))
                bundle_x = (BoundPolyPoints[len(BoundPolyPoints) - 1, 0] +
                            BoundPolyPoints[0, 0]) / 2  #mid-point coordinate x
                bundle_y = (BoundPolyPoints[len(BoundPolyPoints) - 1, 1] +
                            BoundPolyPoints[0, 1]) / 2  #mid-point coordinate y
                q_overlap = hstack((q_overlap, vstack((bundle_x, bundle_y))))
            q_overlap = q_overlap.transpose()
            pt = self.closest_pt([pose[0], pose[1]],
                                 vstack(
                                     (q_overlap,
                                      asarray(PolyUtils.pointList(overlap)))))
            self.prev_follow = pt

            #calculate the vector to follow the obstacle
            normal = mat([pose[0], pose[1]] - pt)

            #find the distance from the closest point
            distance = norm(normal)
            velocity = normal * self.trans_matrix
            vx = (velocity / norm(velocity) / 3)[0, 0]
            vy = (velocity / norm(velocity) / 3)[0, 1]

            # push or pull the robot towards the obstacle depending on whether the robot is close or far from the obstacle.
            turn = pi / 4 * (distance - 0.5 * self.obsRange) / (
                self.obsRange
            )  ### change to 0.6 from 0.5 for more allowance in following
            corr_matrix = mat([[cos(turn), -sin(turn)], [sin(turn),
                                                         cos(turn)]])
            v = corr_matrix * mat([[vx], [vy]])
            vx = v[0, 0]
            vy = v[1, 0]

            ##plotting overlap on figure 2
            if self.PLOT_OVERLAP == True:
                plt.figure(self.overlap_figure)
                plt.clf()
                self.plotPoly(self.m_line, 'b')
                self.plotPoly(overlap, 'r')
                plt.plot(pt[0], pt[1], 'ro')
                plt.plot(pose[0], pose[1], 'bo')
                self.plotPioneer(self.overlap_figure, 0)

            ## conditions that the loop will end
            #for 11111
            RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf + 0.06,
                                          (pose[0], pose[1]))  ####0.05
            departed = not self.currentRegionPoly.overlaps(self.realRobot)
            #departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
            arrived = self.nextRegionPoly.covers(self.realRobot)

            #for 33333
            reachMLine = self.m_line.overlaps(RobotPoly)

            # 1.reached the next region
            if arrived:
                self.boundary_following = False
                self.m_line = None
                self.q_hit_count = 0
                print "arriving at the next region. Exit boundary following mode"
                vx = 0
                vy = 0
                """
                # 2.q_hit is reencountered
                elif norm(self.q_hit-mat([pose[0],pose[1]]).T) < 0.05 and self.q_hit_count > self.q_hit_Thres:
                    print "reencounter q_hit. cannot reach q_goal"
                    vx = 0
                    vy = 0
                """

            # 3.m-line reencountered
            elif reachMLine:
                #print >>sys.__stdout__, "m-line overlaps RoboPoly, m-line" + str(norm(self.q_g-self.q_hit)-2*self.obsRange) + " distance: " + str(norm(self.q_g-mat([pose[0],pose[1]]).T))
                if norm(self.q_g - mat([pose[0], pose[1]]).T
                        ) < norm(self.q_g - self.q_hit) - 2 * self.obsRange:
                    #print "m-line overlaps RoboPoly, m-line" + str(norm(self.q_g-self.q_hit)-2*self.obsRange) + " distance: " + str(norm(self.q_g-mat([pose[0],pose[1]]).T))
                    #print "leaving boundary following mode"
                    self.boundary_following = False
                    self.m_line = None
                    self.q_hit_count = 0
                    leaving = False

                    # turn the robot till it is facing the goal
                    while not leaving:
                        x = self.q_g[0] - self.pose_handler.getPose()[0]
                        y = self.q_g[1] - self.pose_handler.getPose()[1]
                        angle = atan(y / x)
                        if x > 0 and y > 0:
                            angle = angle
                        elif x < 0 and y > 0:
                            angle = pi + angle
                        elif x < 0 and y < 0:
                            angle = pi + angle
                        else:
                            angle = 2 * pi + angle

                        if self.pose_handler.getPose()[2] < 0:
                            omega = (2 * pi + self.pose_handler.getPose()[2])
                            #print >>sys.__stdout__,"omega<0: "+ str(omega)
                        else:
                            omega = self.pose_handler.getPose()[2]
                            #print >>sys.__stdout__,"omega: "+ str(omega)

                        if omega > angle:
                            cc = 2 * pi - (omega - angle)
                        else:
                            cc = angle - omega

                        # angle(goal point orientation) on the left of omega(robot orientation)
                        #if angle - omega > 0 and angle - omega < pi:
                        if cc < pi:
                            #print>>sys.__stdout__, "turn left"
                            vx, vy = self.turnLeft(cc)

                        # on the right
                        else:
                            #print>>sys.__stdout__, "turn right"
                            vx, vy = self.turnRight(2 * pi - cc)

                        #print>>sys.__stdout__, "omega: "+ str(omega) + " angle: "+ str(angle) + " (omega-angle): " + str(omega-angle)
                        self.drive_handler.setVelocity(
                            vx, vy,
                            self.pose_handler.getPose()[2])

                        if omega - angle < pi / 6 and omega - angle > -pi / 6:
                            leaving = True

            #Check whether the robot can leave now (the robot has to be closer to the goal than when it is at q_hit to leave)
            QGoalPoly = PolyShapes.Circle(self.PioneerLengthHalf,
                                          (self.q_g[0], self.q_g[1]))
            path = PolyUtils.convexHull(self.realRobot + QGoalPoly)
            if self.system == 1:
                if self.robocomm.getReceiveObs() == False:
                    pathOverlap = path - (self.map_work)
                else:
                    pathOverlap = path - (self.map_work -
                                          self.robocomm.getObsPoly())
            else:
                pathOverlap = path - (self.map_work)

            if not bool(pathOverlap):
                #print "There is NO MORE obstacles in front for now."

                # check if the robot is closer to the goal compared with q_hit
                if norm(self.q_hit - mat(self.q_g).T) > norm(
                        mat([pose[0], pose[1]]).T - mat(self.q_g).T):
                    #print "The robot is closer than the leaving point. The robot can leave"
                    self.boundary_following = False
                    self.m_line = None
                    self.q_hit_count = 0
                    dis_cur = vstack(
                        (self.q_g[0], self.q_g[1])) - mat([pose[0], pose[1]]).T
                    vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
                    vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
                else:
                    lala = 1
                    #print "not leaving bug algorithm. difference(-farther) =" + str(norm(self.q_hit-mat(self.q_g).T) - norm(mat([pose[0],pose[1]]).T-mat(self.q_g).T))
        """
        # Pass this desired velocity on to the drive handler
        # Check if there are obstacles within 0.35m of the robot, if so, stop the robot
        if self.system == 1:
            if self.robocomm.getSTOP() == True:
                vx = 0
                vy = 0
        """
        #vx = 0
        #vy = 0
        self.overlap = overlap
        self.drive_handler.setVelocity(vx, vy, pose[2])

        # Set the current region as the previous current region(for checking whether the robot has arrived at the next region)
        self.previous_current_reg = current_reg

        # check whether robot has arrived at the next region
        RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf + 0.06,
                                      (pose[0], pose[1]))  ###0.05
        #departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
        departed = not self.currentRegionPoly.overlaps(self.realRobot)
        arrived = self.nextRegionPoly.covers(self.realRobot)
        if arrived:
            self.q_hit_count = 0
            self.boundary_following = False
            self.m_line = None

        if departed and (
                not arrived) and (time.time() - self.last_warning) > 0.5:
            print "WARNING: Left current region but not in expected destination region"
            # Figure out what region we think we stumbled into
            for r in self.proj.rfi.regions:
                pointArray = [self.coordmap_map2lab(x) for x in r.getPoints()]
                vertices = mat(pointArray).T

                if is_inside([pose[0], pose[1]], vertices):
                    #print "I think I'm in " + r.name
                    #print pose
                    break
            self.last_warning = time.time()

        return arrived
Esempio n. 3
0
    def buildTree(self,p,theta,regionPoly,nextRegionPoly,q_gBundle,face_normal, last=False):
        """
        This function builds the RRT tree.
        p                : x,y position of the robot
        theta            : current orientation of the robot
        regionPoly       : current region polygon
        nextRegionPoly   : next region polygon
        q_gBundle        : coordinates of q_goals that the robot can reach
        face_normal      : the normal vector of each face corresponding to each goal point in q_gBundle
        """

        q_init          = mat(p).T
        V               = vstack((0,q_init))
        theta           = self.orientation_bound(theta)
        V_theta         = array([theta])

        #!!! CONTROL SPACE: generate a list of omega for random sampling
        omegaLowerBound = -math.pi/20      # upper bound for the value of omega
        omegaUpperBound = math.pi/20       # lower bound for the value of omega
        omegaNoOfSteps  = 20
        self.omega_range = linspace(omegaLowerBound,omegaUpperBound,omegaNoOfSteps)
        self.omega_range_escape = linspace(omegaLowerBound*4,omegaUpperBound*4,omegaNoOfSteps*4)    # range used when stuck > stuck_thres

        regionPolyOld = Polygon.Polygon(regionPoly)
        regionPoly += PolyShapes.Circle(self.radius*2.5,(q_init[0,0],q_init[1,0]))

        # check faces of the current region for goal points
        E     = [[],[]]       # the tree matrix
        Other = [[],[]]
        path                        = False          # if path formed then = 1
        stuck                       = 0              # count for changing the range of sampling omega
        append_after_latest_node    = False       # append new nodes to the latest node

        if self.system_print == True:
            print "plotting in buildTree is " + str(self.plotting)
        if self.plotting == True:
            if not plt.isinteractive():
                plt.ion()
            plt.hold(True)


        while not path:
            #step -1: try connection to q_goal (generate path to goal)
            i = 0

            if self.system_print == True:
                print "Try Connection to the goal points"

            # pushing possible q_goals into the current region (ensure path is covered by the current region polygon)
            q_pass = [[],[],[]]
            q_pass_dist = []
            q_gBundle = mat(q_gBundle)
            face_normal = mat(face_normal)

            while i < q_gBundle.shape[1]:
                q_g_original = q_gBundle[:,i]
                q_g = q_gBundle[:,i]+face_normal[:,i]*1.5*self.radius    ##original 2*self.radius
                #q_g = q_gBundle[:,i]+(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*1.5*self.radius    ##original 2*self.radius
                if not regionPolyOld.isInside(q_g[0],q_g[1]):
                    #q_g = q_gBundle[:,i]-(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*1.5*self.radius    ##original 2*self.radius
                    q_g = q_gBundle[:,i]-face_normal[:,i]*1.5*self.radius    ##original 2*self.radius

                #forming polygon for path checking
                EdgePolyGoal = PolyShapes.Circle(self.radius,(q_g[0,0],q_g[1,0])) + PolyShapes.Circle(self.radius,(V[1,shape(V)[1]-1],V[2:,shape(V)[1]-1]))
                EdgePolyGoal = PolyUtils.convexHull(EdgePolyGoal)
                dist = norm(q_g - V[1:,shape(V)[1]-1])

                #check connection to goal
                connect_goal = regionPoly.covers(EdgePolyGoal)   #check coverage of path from new point to goal

                # compare orientation difference
                thetaPrev = V_theta[shape(V)[1]-1]

                theta_orientation = abs(arctan((q_g[1,0]- V[2,shape(V)[1]-1])/(q_g[0,0]- V[1,shape(V)[1]-1])))
                if q_g[1,0] > V[2,shape(V)[1]-1]:
                    if q_g[0,0] < V[1,shape(V)[1]-1]: # second quadrant
                        theta_orientation = pi - theta_orientation
                    elif q_g[0,0] > V[1,shape(V)[1]-1]: # first quadrant
                        theta_orientation = theta_orientation
                elif q_g[1,0] < V[2,shape(V)[1]-1]:
                    if q_g[0,0] < V[1,shape(V)[1]-1]: #third quadrant
                        theta_orientation = pi + theta_orientation
                    elif q_g[0,0] > V[1,shape(V)[1]-1]: # foruth quadrant
                        theta_orientation =  2*pi - theta_orientation

                # check the angle between vector(new goal to goal_original ) and vector( latest node to new goal)
                Goal_to_GoalOriginal = q_g_original - q_g
                LatestNode_to_Goal   = q_g - V[1:,shape(V)[1]-1]
                Angle_Goal_LatestNode= arccos(vdot(array(Goal_to_GoalOriginal), array(LatestNode_to_Goal))/norm(Goal_to_GoalOriginal)/norm(LatestNode_to_Goal))

                # if connection to goal can be established and the max change in orientation of the robot is smaller than max_angle, tree is said to be completed.
                if self.orientation_print == True:
                    print "theta_orientation is " + str(theta_orientation)
                    print "thetaPrev is " + str(thetaPrev)
                    print "(theta_orientation - thetaPrev) is " + str(abs(theta_orientation - thetaPrev))
                    print "self.max_angle_allowed is " + str(self.max_angle_allowed)
                    print "abs(theta_orientation - thetaPrev) < self.max_angle_allowed" + str(abs(theta_orientation - thetaPrev) < self.max_angle_allowed)
                    print"Goal_to_GoalOriginal: " + str( array(Goal_to_GoalOriginal)) + "; LatestNode_to_Goal: " + str( array(LatestNode_to_Goal))
                    print vdot(array(Goal_to_GoalOriginal), array(LatestNode_to_Goal))
                    print "Angle_Goal_LatestNode" + str(Angle_Goal_LatestNode)

                if connect_goal and (abs(theta_orientation - thetaPrev) < self.max_angle_allowed) and (Angle_Goal_LatestNode < self.max_angle_allowed):

                    path = True
                    q_pass = hstack((q_pass,vstack((i,q_g))))
                    q_pass_dist = hstack((q_pass_dist,dist))

                i = i + 1

            if self.system_print == True:
                print "checked goal points"

            self.E = E
            self.V = V
            # connection to goal has established
            # Obtain the closest goal point that path can be formed.
            if path:
                if shape(q_pass_dist)[0] == 1:
                    cols = 0
                else:
                    (cols,) = nonzero(q_pass_dist == min(q_pass_dist))
                    cols = asarray(cols)[0]
                q_g = q_pass[1:,cols]
                """
                q_g = q_g-(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*3*self.radius   #org 3
                if not nextRegionPoly.isInside(q_g[0],q_g[1]):
                    q_g = q_g+(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*6*self.radius   #org 3
                """
                if self.plotting == True :
                    if self.operate_system == 1:
                        plt.suptitle('Rapidly-exploring Random Tree', fontsize=12)
                        plt.xlabel('x')
                        plt.ylabel('y')
                        if shape(V)[1] <= 2:
                            plt.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        else:
                            plt.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        plt.plot(q_g[0,0],q_g[1,0],'ko')
                        plt.figure(1).canvas.draw()
                    else:
                        BoundPolyPoints = asarray(PolyUtils.pointList(regionPoly))
                        self.ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
                        if shape(V)[1] <= 2:
                            self.ax.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        else:
                            self.ax.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        self.ax.plot(q_g[0,0],q_g[1,0],'ko')

                # trim the path connecting current node to goal point into pieces if the path is too long now
                numOfPoint = floor(norm(V[1:,shape(V)[1]-1]- q_g)/self.step_size)
                if numOfPoint < 3:
                    numOfPoint = 3
                x = linspace( V[1,shape(V)[1]-1], q_g[0,0], numOfPoint )
                y = linspace( V[2,shape(V)[1]-1], q_g[1,0], numOfPoint )
                for i in range(x.shape[0]):
                    if i != 0:
                        V = hstack((V,vstack((shape(V)[1],x[i],y[i]))))
                        E = hstack((E,vstack((shape(V)[1]-2,shape(V)[1]-1))))

                #push the goal point to the next region
                q_g = q_g+face_normal[:,q_pass[0,cols]]*3*self.radius    ##original 2*self.radius
                if not nextRegionPoly.isInside(q_g[0],q_g[1]):
                    q_g = q_g-face_normal[:,q_pass[0,cols]]*6*self.radius    ##original 2*self.radius
                V = hstack((V,vstack((shape(V)[1],q_g[0,0],q_g[1,0]))))
                E = hstack((E,vstack((shape(V)[1]-2 ,shape(V)[1]-1))))

                if self.plotting == True :
                    if self.operate_system == 1:
                        plt.plot(q_g[0,0],q_g[1,0],'ko')
                        plt.plot(( V[1,shape(V)[1]-1],V[1,shape(V)[1]-2]),( V[2,shape(V)[1]-1],V[2,shape(V)[1]-2]),'b')
                        plt.figure(1).canvas.draw()
                    else:
                        self.ax.plot(q_g[0,0],q_g[1,0],'ko')
                        self.ax.plot(( V[1,shape(V)[1]-1],V[1,shape(V)[1]-2]),( V[2,shape(V)[1]-1],V[2,shape(V)[1]-2]),'b')

            # path is not formed, try to append points onto the tree
            if not path:

                # connection_to_tree : connection to the tree is successful
                if append_after_latest_node:
                    V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree = self.generateNewNode(V,V_theta,E,Other,regionPoly,stuck, append_after_latest_node)
                else:
                    connection_to_tree = False
                    while not connection_to_tree:
                        V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree = self.generateNewNode  (V,V_theta,E,Other,regionPoly,stuck)


        if self.finish_print:
            print 'Here is the V matrix:', V, 'Here is the E matrix:',E
            print >>sys.__stdout__, 'Here is the V matrix:\n', V, '\nHere is the E matrix:\n',E

        #B: trim to a single path
        single = 0
        while single == 0:
            trim  = 0
            for j in range(shape(V)[1]-3):
                (row,col) = nonzero(E == j+1)
                if len(col) == 1:
                    E = delete(E, col[0], 1)
                    trim = 1

            if trim == 0:
                single = 1;

        ####print with matlib
        if self.plotting ==True :
            if self.operate_system == 1:
                plt.plot(V[1,:],V[2,:],'b')
                for i in range(shape(E)[1]):
                    plt.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
                    plt.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)
                plt.figure(1).canvas.draw()
            else:
                BoundPolyPoints = asarray(PolyUtils.pointList(regionPoly))
                self.ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
                self.ax.plot(V[1,:],V[2,:],'b')
                for i in range(shape(E)[1]):
                    self.ax.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
                    self.ax.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)

        #return V, E, and the current node number on the tree
        V = array(V)
        return V, E, 0
Esempio n. 4
0
    def gotoRegion(self, current_reg, next_reg, last=False):

        """
        If ``last`` is True, we will move to the center of the destination region.
        Returns ``True`` if we've reached the destination region.
        """
        q_gBundle              = [[],[]]          # goal bundle
        q_overlap              = [[],[]]          # overlapping points with robot range
        pose = self.pose_handler.getPose()        # Find our current configuration

        #Plot the robot on the map in figure 1
        if self.PLOT == True:
            plt.figure(self.original_figure)
            plt.clf()
            self.plotPoly(self.realRobot, 'r')
            self.plotPoly(self.robot, 'b')
            plt.plot(pose[0],pose[1],'bo')
            self.plotPioneer(self.original_figure)


        if current_reg == next_reg and not last:
            # No need to move!
            self.drive_handler.setVelocity(0, 0)  # So let's stop
            return False

        # Check if Vicon has cut out
        if math.isnan(pose[2]):
            print "no vicon pose"
            print "WARNING: No Vicon data! Pausing."
            #self.drive_handler.setVelocity(0, 0)  # So let's stop
            time.sleep(1)
            return False

        ###This part is run when the robot goes to a new region, otherwise, the original map will be used.
        if not self.previous_current_reg == current_reg:
            #print 'getting into bug alogorithm'

            #clean up the previous self.map_work
            self.map_work =  Polygon.Polygon()

            # NOTE: Information about region geometry can be found in self.proj.rfi.regions
            # create polygon list for regions other than the current_reg and the next_reg
            self.map_work += self.map[self.proj.rfi.regions[current_reg].name]
            self.map_work += self.map[self.proj.rfi.regions[next_reg].name]

            # building current polygon and destination polygon
            self.nextRegionPoly    = self.map[self.proj.rfi.regions[next_reg].name]
            self.currentRegionPoly = self.map[self.proj.rfi.regions[current_reg].name]

            #set to zero velocity before finding the tranFace
            self.drive_handler.setVelocity(0, 0)
            if last:
                transFace = None
            else:
                print "Current reg is " + str(self.proj.rfi.regions[current_reg].name.lower())
                print "Next reg is "+ str(self.proj.rfi.regions[next_reg].name.lower())


                for i in range(len(self.proj.rfi.transitions[current_reg][next_reg])):
                    pointArray_transface = [x for x in self.proj.rfi.transitions[current_reg][next_reg][i]]
                    transFace = asarray(map(self.coordmap_map2lab,pointArray_transface))
                    bundle_x = (transFace[0,0] +transFace[1,0])/2    #mid-point coordinate x
                    bundle_y = (transFace[0,1] +transFace[1,1])/2    #mid-point coordinate y
                    q_gBundle = hstack((q_gBundle,vstack((bundle_x,bundle_y))))
                q_gBundle = q_gBundle.transpose()

                # Find the closest face to the current position
                max_magsq = 1000000
                for tf in q_gBundle:
                    magsq = (tf[0] - pose[0])**2 + (tf[1] - pose[1])**2
                    if magsq < max_magsq:
                        connection = 0
                        tf = tf+(tf-asarray(self.currentRegionPoly.center()))/norm(tf-asarray(self.currentRegionPoly.center()))*2.1*self.PioneerLengthHalf
                        if not self.nextRegionPoly.covers(PolyShapes.Circle(self.PioneerLengthHalf*2,(tf[0],tf[1]))):
                            tf = tf-(tf-asarray(self.currentRegionPoly.center()))/norm(tf-asarray(self.currentRegionPoly.center()))*4.2*self.PioneerLengthHalf
                            if self.nextRegionPoly.covers(PolyShapes.Circle(self.PioneerLengthHalf*2,(tf[0],tf[1]))):
                                connection = 1
                        else:
                            connection = 1
                        if connection == 1:
                            pt1  = tf
                            max_magsq = magsq
                            transFace = 1
                            self.q_g[0] = pt1[0]
                            self.q_g[1] = pt1[1]
                        else:
                            sample = False
                            while not sample:
                                self.q_g[0],self.q_g[1] = self.nextRegionPoly.sample(random.random)
                                robo = PolyShapes.Circle(self.PioneerLengthHalf,(self.q_g[0],self.q_g[1]))
                                if not bool(robo - self.nextRegionPoly):
                                    sample = True


                """
                # Push the goal point to somewhere inside the next region to ensure the robot will get there.(CHECK!!)
                self.q_g = self.q_g+(self.q_g-asarray(self.currentRegionPoly.center()))/norm(self.q_g-asarray(self.currentRegionPoly.center()))*3*self.PioneerLengthHalf
                if not self.nextRegionPoly.isInside(self.q_g[0],self.q_g[1]):
                    self.q_g = self.q_g-(self.q_g-asarray(self.currentRegionPoly.center()))/norm(self.q_g-asarray(self.currentRegionPoly.center()))*6*self.PioneerLengthHalf
                """

                #plot exiting point
                if self.PLOT_EXIT == True:
                    plt.figure(self.overlap_figure)
                    plt.clf()
                    plt.plot(q_gBundle[:,0],q_gBundle[:,1],'ko' )
                    plt.plot(self.q_g[0],self.q_g[1],'ro')
                    plt.plot(pose[0],pose[1],'bo')
                    self.plotPioneer(self.overlap_figure,0)

                if transFace is None:
                    print "ERROR: Unable to find transition face between regions %s and %s.  Please check the decomposition (try viewing projectname_decomposed.regions in RegionEditor or a text editor)." % (self.proj.rfi.regions[current_reg].name, self.proj.rfi.regions[next_reg].name)


        ##################################################
        #######check whether obstacle is detected#########
        ##################################################

        #Update pose,update self.robot, self.realRobot orientation
        self.robot.shift(pose[0]-self.prev_pose[0],pose[1]-self.prev_pose[1])
        self.realRobot.shift(pose[0]-self.prev_pose[0],pose[1]-self.prev_pose[1])
        self.robot.rotate(pose[2]-self.prev_pose[2],pose[0],pose[1])
        self.realRobot.rotate(pose[2]-self.prev_pose[2],pose[0],pose[1])
        self.prev_pose = pose

        ############################
        ########### STEP 1##########
        ############################
        ##Check whether obsRange overlaps with obstacle or the boundary (overlap returns the part of robot not covered by the region

        # for real Pioneer robot
        if self.system == 1:
            # motion controller is not in boundary following mode
            if self.boundary_following == False:

                if self.robocomm.getReceiveObs() == False:
                    overlap = self.robot - ( self.map_work)
                else:
                    overlap = self.robot  - ( self.map_work - self.robocomm.getObsPoly())

            else: #TRUE
                # use a robot with full range all around it
                Robot = PolyShapes.Circle(self.obsRange,(pose[0],pose[1]))
                Robot.shift(self.shift*cos(pose[2]),self.shift*sin(pose[2]))

                if self.robocomm.getReceiveObs() == False:
                    overlap = Robot - ( self.map_work)
                else:
                    overlap = Robot  - ( self.map_work - self.robocomm.getObsPoly())
        # for ODE
        else:
            if self.boundary_following == False:
                overlap = self.robot  - (self.map_work)
            else:#TRUE
                overlap = self.robot  - (self.map_work)

        if self.boundary_following == False:
            if bool(overlap):    ## overlap of obstacles
                #print "There MAYBE overlap~~ check connection to goal"

                # check whether the real robot or and path to goal overlap with the obstacle
                QGoalPoly= PolyShapes.Circle(self.PioneerLengthHalf,(self.q_g[0],self.q_g[1]))
                path  = PolyUtils.convexHull(self.realRobot + QGoalPoly)
                if self.system == 1:
                    if self.robocomm.getReceiveObs() == False:
                        pathOverlap = path - ( self.map_work)
                    else:
                        pathOverlap = path  - ( self.map_work - self.robocomm.getObsPoly())
                else:
                    pathOverlap = path - ( self.map_work)


                if bool(pathOverlap):   # there is overlapping, go into bounding following mode
                    #print "There IS overlap"
                    self.q_hit  = mat([pose[0],pose[1]]).T
                    self.boundary_following = True

                    #Generate m-line polygon
                    QHitPoly = PolyShapes.Circle(self.PioneerLengthHalf/4,(pose[0],pose[1]))
                    QGoalPoly= PolyShapes.Circle(self.PioneerLengthHalf/4,(self.q_g[0],self.q_g[1]))
                    self.m_line  = PolyUtils.convexHull(QHitPoly + QGoalPoly)

                    #plot the first overlap
                    if self.PLOT_M_LINE == True:
                        plt.figure(self.overlap_figure)
                        plt.clf()
                        self.plotPoly(QHitPoly,'k')
                        self.plotPoly(QGoalPoly,'k')
                        self.plotPoly(overlap,'g')
                        self.plotPoly(self.m_line,'b')
                        plt.plot(pose[0],pose[1],'bo')
                        self.plotPioneer(self.overlap_figure,0)


                else:                ##head towards the q_goal
                    if self.system == 1:
                        if self.robocomm.getReceiveObs() == False:   # wait for obstacles from Pioneer
                            vx = 0
                            vy = 0
                        else:
                            dis_cur  = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
                            vx = (dis_cur/norm(dis_cur)/3)[0,0]
                            vy = (dis_cur/norm(dis_cur)/3)[1,0]

                    else:
                        dis_cur  = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
                        vx = (dis_cur/norm(dis_cur)/3)[0,0]
                        vy = (dis_cur/norm(dis_cur)/3)[1,0]
                        #print "no obstacles 2-ODE true"

            else:                ##head towards the q_goal
                    if self.system == 1:
                        if self.robocomm.getReceiveObs() == False:   # wait for obstacles from Pioneer
                            vx = 0
                            vy = 0
                        else:
                            dis_cur  = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
                            vx = (dis_cur/norm(dis_cur)/3)[0,0]
                            vy = (dis_cur/norm(dis_cur)/3)[1,0]

                    else:
                        dis_cur  = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
                        vx = (dis_cur/norm(dis_cur)/3)[0,0]
                        vy = (dis_cur/norm(dis_cur)/3)[1,0]
                        #print "no obstacles 1-ODE true"

        if self.boundary_following == True:
            self.q_hit_count       += 1
            # finding the point to go normal to (closest overlapping point)

            j = 0
            recheck = 0
            while not bool(overlap):
                # cannot see the obstacle. Based on the location of the previous point blow up the range of the robot on the left or on the right
                j += 1

                # finding whether the previous obstacle point is on the left side or the right side of the robot
                # angle = angle of the previous point from the x-axis of the field
                # omega = angle of the current Pioneer orientation from the x-axis of the field
                # cc    = differnece between angle and omega ( < pi = previous point on the left of robot, else on the right of robot)
                x = self.prev_follow[0] -pose[0]
                y = self.prev_follow[1] -pose[1]
                angle = atan(y/x)

                # convert angle to 2pi
                if x > 0 and y > 0:
                    angle = angle
                elif x < 0 and y > 0:
                    angle = pi + angle
                elif x <0 and y < 0:
                    angle = pi + angle
                else:
                    angle = 2*pi + angle

                # convert pose to 2pi
                if pose[2] < 0:
                    omega = (2*pi + pose[2])
                else:
                    omega = pose[2]


                if omega > angle:
                    cc = 2*pi - (omega - angle)
                else:
                    cc = angle - omega

                # on the left
                #if angle - omega > 0 and angle - omega < pi:
                if cc < pi:
                    #print "on the left, angle: "+ str(angle) + " omega: "+ str(omega)+ " angle-omega: "+ str(angle-omega)
                    Robot = PolyShapes.Rectangle(self.range*2*j,self.range*2*j)
                    Robot.shift(pose[0]-self.range*j*2,pose[1]-self.range*j)
                    Robot.rotate(pose[2]-pi/2,pose[0],pose[1])

                # on the right
                else:
                    #print "on the right, angle: "+ str(angle) + " omega: "+ str(omega)+ " angle-omega: "+ str(angle-omega)
                    Robot = PolyShapes.Rectangle(self.range*2*j,self.range*2*j)
                    Robot.shift(pose[0],pose[1]-self.range*j)
                    Robot.rotate(pose[2]-pi/2,pose[0],pose[1])

                if self.system == 1:
                    overlap = Robot - ( self.map_work - self.robocomm.getObsPoly())
                else:
                    overlap = Robot - ( self.map_work)

                #self.plotPoly(Robot, 'm',2)
                #determines as dynamic obstacles and can be go striaight to the goal point
                if j >= 2:
                    dis_cur  = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
                    vx = (dis_cur/norm(dis_cur)/3)[0,0]
                    vy = (dis_cur/norm(dis_cur)/3)[1,0]
                    overlap = None
                    self.overlap = overlap
                    self.q_hit_count        = 0
                    self.boundary_following = False
                    self.m_line             = None
                    self.drive_handler.setVelocity(vx,vy, pose[2])
                    RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf+0.06,(pose[0],pose[1]))     ###0.05
                    departed = not self.currentRegionPoly.overlaps(self.realRobot)
                    #departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
                    arrived  = self.nextRegionPoly.covers(self.realRobot)
                    return arrived




            ##extra box plotting in figure 1#
            if self.PLOT_OVERLAP == True:
                plt.figure(self.original_figure)
                plt.clf()
                self.plotPoly(self.realRobot, 'r')
                self.plotPoly(self.robot, 'b')
                self.plotPoly(overlap,'g',3)
                plt.plot(pose[0],pose[1],'bo')
                self.plotPioneer(self.original_figure)

            # find the closest point on the obstacle to the robot
            overlap_len = len(overlap)
            for j in range(overlap_len):
                BoundPolyPoints = asarray(overlap[j])
                for i in range(len(BoundPolyPoints)-1):
                    bundle_x = (BoundPolyPoints[i,0] +BoundPolyPoints[1+i,0])/2    #mid-point coordinate x
                    bundle_y = (BoundPolyPoints[i,1] +BoundPolyPoints[1+i,1])/2    #mid-point coordinate y
                    q_overlap = hstack((q_overlap,vstack((bundle_x,bundle_y))))
                bundle_x = (BoundPolyPoints[len(BoundPolyPoints)-1,0] +BoundPolyPoints[0,0])/2    #mid-point coordinate x
                bundle_y = (BoundPolyPoints[len(BoundPolyPoints)-1,1] +BoundPolyPoints[0,1])/2    #mid-point coordinate y
                q_overlap = hstack((q_overlap,vstack((bundle_x,bundle_y))))
            q_overlap = q_overlap.transpose()
            pt =  self.closest_pt([pose[0],pose[1]], vstack((q_overlap,asarray(PolyUtils.pointList(overlap)))))
            self.prev_follow = pt

            #calculate the vector to follow the obstacle
            normal = mat([pose[0],pose[1]] - pt)

            #find the distance from the closest point
            distance = norm(normal)
            velocity = normal * self.trans_matrix
            vx = (velocity/norm(velocity)/3)[0,0]
            vy = (velocity/norm(velocity)/3)[0,1]

            # push or pull the robot towards the obstacle depending on whether the robot is close or far from the obstacle.
            turn = pi/4*(distance-0.5*self.obsRange)/(self.obsRange)    ### change to 0.6 from 0.5 for more allowance in following
            corr_matrix       = mat([[cos(turn),-sin(turn)],[sin(turn),cos(turn)]])
            v =  corr_matrix*mat([[vx],[vy]])
            vx = v[0,0]
            vy = v[1,0]


            ##plotting overlap on figure 2
            if self.PLOT_OVERLAP == True:
                plt.figure(self.overlap_figure)
                plt.clf()
                self.plotPoly(self.m_line,'b');
                self.plotPoly(overlap,'r');
                plt.plot(pt[0],pt[1],'ro')
                plt.plot(pose[0],pose[1],'bo')
                self.plotPioneer(self.overlap_figure,0)


            ## conditions that the loop will end
            #for 11111
            RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf+0.06,(pose[0],pose[1]))   ####0.05
            departed = not self.currentRegionPoly.overlaps(self.realRobot)
            #departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
            arrived  = self.nextRegionPoly.covers(self.realRobot)

            #for 33333
            reachMLine= self.m_line.overlaps(RobotPoly)

            # 1.reached the next region
            if arrived:
                self.boundary_following = False
                self.m_line             = None
                self.q_hit_count       = 0
                print "arriving at the next region. Exit boundary following mode"
                vx = 0
                vy = 0

                """
                # 2.q_hit is reencountered
                elif norm(self.q_hit-mat([pose[0],pose[1]]).T) < 0.05 and self.q_hit_count > self.q_hit_Thres:
                    print "reencounter q_hit. cannot reach q_goal"
                    vx = 0
                    vy = 0
                """

            # 3.m-line reencountered
            elif reachMLine:
                #print >>sys.__stdout__, "m-line overlaps RoboPoly, m-line" + str(norm(self.q_g-self.q_hit)-2*self.obsRange) + " distance: " + str(norm(self.q_g-mat([pose[0],pose[1]]).T))
                if norm(self.q_g-mat([pose[0],pose[1]]).T) < norm(self.q_g-self.q_hit)-2*self.obsRange:
                    #print "m-line overlaps RoboPoly, m-line" + str(norm(self.q_g-self.q_hit)-2*self.obsRange) + " distance: " + str(norm(self.q_g-mat([pose[0],pose[1]]).T))
                    #print "leaving boundary following mode"
                    self.boundary_following = False
                    self.m_line             = None
                    self.q_hit_count       = 0
                    leaving                = False

                    # turn the robot till it is facing the goal
                    while not leaving:
                        x = self.q_g[0] -self.pose_handler.getPose()[0]
                        y = self.q_g[1] -self.pose_handler.getPose()[1]
                        angle = atan(y/x)
                        if x > 0 and y > 0:
                            angle = angle
                        elif x < 0 and y > 0:
                            angle = pi + angle
                        elif x <0 and y < 0:
                            angle = pi + angle
                        else:
                            angle = 2*pi + angle


                        if self.pose_handler.getPose()[2] < 0:
                            omega = (2*pi + self.pose_handler.getPose()[2])
                            #print >>sys.__stdout__,"omega<0: "+ str(omega)
                        else:
                            omega = self.pose_handler.getPose()[2]
                            #print >>sys.__stdout__,"omega: "+ str(omega)


                        if omega > angle:
                            cc = 2*pi - (omega - angle)
                        else:
                            cc = angle - omega

                        # angle(goal point orientation) on the left of omega(robot orientation)
                        #if angle - omega > 0 and angle - omega < pi:
                        if cc < pi:
                            #print>>sys.__stdout__, "turn left"
                            vx,vy = self.turnLeft(cc)

                        # on the right
                        else:
                            #print>>sys.__stdout__, "turn right"
                            vx, vy = self.turnRight(2*pi-cc)

                        #print>>sys.__stdout__, "omega: "+ str(omega) + " angle: "+ str(angle) + " (omega-angle): " + str(omega-angle)
                        self.drive_handler.setVelocity(vx,vy, self.pose_handler.getPose()[2])

                        if omega - angle < pi/6 and omega - angle > -pi/6:
                            leaving = True

            #Check whether the robot can leave now (the robot has to be closer to the goal than when it is at q_hit to leave)
            QGoalPoly= PolyShapes.Circle(self.PioneerLengthHalf,(self.q_g[0],self.q_g[1]))
            path  = PolyUtils.convexHull(self.realRobot + QGoalPoly)
            if self.system == 1:
                if self.robocomm.getReceiveObs() == False:
                    pathOverlap = path - ( self.map_work)
                else:
                    pathOverlap = path  - ( self.map_work - self.robocomm.getObsPoly())
            else:
                pathOverlap = path - ( self.map_work)

            if not bool(pathOverlap):
                #print "There is NO MORE obstacles in front for now."

                # check if the robot is closer to the goal compared with q_hit
                if norm(self.q_hit-mat(self.q_g).T) > norm(mat([pose[0],pose[1]]).T-mat(self.q_g).T) :
                    #print "The robot is closer than the leaving point. The robot can leave"
                    self.boundary_following = False
                    self.m_line             = None
                    self.q_hit_count       = 0
                    dis_cur  = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
                    vx = (dis_cur/norm(dis_cur)/3)[0,0]
                    vy = (dis_cur/norm(dis_cur)/3)[1,0]
                else:
                    lala = 1
                    #print "not leaving bug algorithm. difference(-farther) =" + str(norm(self.q_hit-mat(self.q_g).T) - norm(mat([pose[0],pose[1]]).T-mat(self.q_g).T))

        """
        # Pass this desired velocity on to the drive handler
        # Check if there are obstacles within 0.35m of the robot, if so, stop the robot
        if self.system == 1:
            if self.robocomm.getSTOP() == True:
                vx = 0
                vy = 0
        """
        #vx = 0
        #vy = 0
        self.overlap = overlap
        self.drive_handler.setVelocity(vx,vy, pose[2])


        # Set the current region as the previous current region(for checking whether the robot has arrived at the next region)
        self.previous_current_reg = current_reg


        # check whether robot has arrived at the next region
        RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf+0.06,(pose[0],pose[1]))     ###0.05
        #departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
        departed = not self.currentRegionPoly.overlaps(self.realRobot)
        arrived  = self.nextRegionPoly.covers(self.realRobot)
        if arrived:
            self.q_hit_count        = 0
            self.boundary_following = False
            self.m_line             = None

        if departed and (not arrived) and (time.time()-self.last_warning) > 0.5:
            print "WARNING: Left current region but not in expected destination region"
            # Figure out what region we think we stumbled into
            for r in self.proj.rfi.regions:
                pointArray = [self.coordmap_map2lab(x) for x in r.getPoints()]
                vertices = mat(pointArray).T

                if is_inside([pose[0], pose[1]], vertices):
                    #print "I think I'm in " + r.name
                    #print pose
                    break
            self.last_warning = time.time()

        return arrived
Esempio n. 5
0
def buildTree(p,theta,vert, R, system, regionPoly,nextRegionPoly,q_gBundle,\
mappedRegions,allRegions,max_angle_allowed, plotting, operate_system,ax, last=False):
    """
    This function builds the RRT tree
    pose: x,y position of the robot
    theta: current orientation of the robot
    R: radius of the robot
    system: determine step_size and radius for the example
    regionPoly: current region polygon
    nextRegionPoly: next region polygon
    q_gBundle: coordinates of q_goals that the robot can reach
    mappedRegions: region polygons
    allRegions: polygon that includes all the region
    max_angle_allowed: the difference in angle between the two nodes allowed (between 0 to 2*pi)
    plotting: 1 if plotting is enabled, 0- disabled
    operate_system: Which operating system is used for execution. Ubuntu and Mac is 1, Windows is 2
    ax: plot for windows
    """

    max_angle = max_angle_allowed
    """
    if operate_system == 2:
        # start using anmination to plot Pioneer
        self.fig = plt.figure()
        self.ax = self.fig.add_subplot(111)
        self.scope = _Scope(self.ax,self)
        thread.start_new_thread(self.jplot,())
    """

    ## 1: Nao ; 2: STAGE ;  3: ODE
    if system == 1:  ## Nao
        step_size = 0.2  #set the step_size for points be 1/5 of the norm from   ORIGINAL = 0.4
    elif system == 2:
        step_size = 0.5
    elif system == 3:
        step_size = 15

    if system == 1:  ## Nao
        timeStep = 5  #time step for calculation of x, y position
    elif system == 2:
        timeStep = 4  #time step for calculation of x, y position
    elif system == 3:
        timeStep = 10  #time step for calculation of x, y position    #10
    #############tune velocity OMEGA, TIME STEP

    #fix velocity
    ## 1: Nao ; 2: STAGE ;  3: ODE
    if system == 1:  ## Nao
        velocity = 0.05  #set the step_size for points be 1/5 of the norm from
    elif system == 2:
        velocity = 0.06
    elif system == 3:
        velocity = 2  # what is used in RRTControllerHelper.setVelocity    #1.5
    #############tune velocity OMEGA, TIME STEP

    BoundPoly = regionPoly  # Boundary polygon = current region polygon
    radius = R
    q_init = mat(p).T
    V = vstack((0, q_init))
    V_theta = array([theta])
    original_figure = 1

    #!!! CONTROL SPACE: generate a list of omega for random sampling
    omegaLowerBound = -math.pi / 20  # upper bound for the value of omega
    omegaUpperBound = math.pi / 20  # lower bound for the value of omega
    omegaStepSize = 20
    omega_range = linspace(omegaLowerBound, omegaUpperBound, omegaStepSize)
    omega_range_abso = linspace(omegaLowerBound * 4, omegaUpperBound * 4,
                                omegaStepSize *
                                4)  # range used when stuck > stuck_thres
    edgeX = []
    edgeY = []

    # check faces of the current region for goal points

    E = [[], []]
    Other = [[], []]
    path = 0  # if path formed then = 1
    stuck = 0  # count for changing the range of sampling omega
    stuck_thres = 300  # threshold for changing the range of sampling omega

    if not plt.isinteractive():
        plt.ion()
    plt.hold(True)
    while path == 0:
        #step -1: try connection to q_goal (generate path to goal)
        goalCheck = 0
        i = 0

        # pushing possible q_goals into the current region (ensure path is covered by the current region polygon)
        q_pass = [[], [], []]
        q_pass_dist = []
        q_gBundle = mat(q_gBundle)
        while i < q_gBundle.shape[1]:  ###not sure about shape
            q_g = q_gBundle[:, i] + (q_gBundle[:, i] - V[1:, (
                shape(V)[1] - 1)]) / norm(q_gBundle[:, i] - V[1:, (
                    shape(V)[1] - 1)]) * radius  ##original 2*radius
            trial = 1
            if not BoundPoly.isInside(q_g[0], q_g[1]):
                trial = 2
                q_g = q_gBundle[:, i] - (q_gBundle[:, i] - V[1:, (
                    shape(V)[1] - 1)]) / norm(q_gBundle[:, i] - V[1:, (
                        shape(V)[1] - 1)]) * radius  ##original 2*radius

            #forming polygon for path checking
            cross_goal = cross(
                vstack((q_g - vstack(
                    (V[1, shape(V)[1] - 1], V[2, shape(V)[1] - 1])), 0)).T,
                hstack((0, 0, 1)))
            cross_goal = cross_goal.T
            move_vector_goal = radius * cross_goal[0:2] / sqrt(
                (cross_goal[0, 0]**2 + cross_goal[1, 0]**2))
            upperEdgeG = hstack((vstack(
                (V[1,
                   shape(V)[1] - 1], V[2, shape(V)[1] - 1])), q_g)) + hstack(
                       (move_vector_goal, move_vector_goal))
            lowerEdgeG = hstack((vstack(
                (V[1,
                   shape(V)[1] - 1], V[2, shape(V)[1] - 1])), q_g)) - hstack(
                       (move_vector_goal, move_vector_goal))
            EdgePolyGoal = Polygon.Polygon(
                (tuple(array(lowerEdgeG[:, 0].T)[0]),
                 tuple(array(upperEdgeG[:, 0].T)[0]),
                 tuple(array(upperEdgeG[:, 1].T)[0]),
                 tuple(array(lowerEdgeG[:, 1].T)[0])))

            dist = norm(q_g - V[1:, shape(V)[1] - 1])
            connect_goal = BoundPoly.covers(
                EdgePolyGoal)  #check coverage of path from new point to goal
            #check connection to goal
            """
            if connect_goal:
                print "connection is true"
                path = 1
                q_pass = hstack((q_pass,vstack((i,q_g))))
                q_pass_dist = hstack((q_pass_dist,dist))
            """

            # compare orientation difference
            thetaPrev = V_theta[shape(V)[1] - 1]
            theta_orientation = abs(
                arctan((q_g[1, 0] - V[2, shape(V)[1] - 1]) /
                       (q_g[0, 0] - V[1, shape(V)[1] - 1])))
            if thetaPrev < 0:
                if q_g[1, 0] > V[2, shape(V)[1] - 1]:
                    if q_g[0, 0] < V[1, shape(V)[1] - 1]:  # second quadrant
                        theta_orientation = -2 * pi + theta_orientation
                    elif q_g[0, 0] > V[1, shape(V)[1] - 1]:  # first quadrant
                        theta_orientation = -pi - theta_orientation
                elif q_g[1, 0] < V[2, shape(V)[1] - 1]:
                    if q_g[0, 0] < V[1, shape(V)[1] - 1]:  #third quadrant
                        theta_orientation = -pi + theta_orientation
                    elif q_g[0, 0] > V[1, shape(V)[1] - 1]:  # foruth quadrant
                        theta_orientation = -theta_orientation
            else:
                if q_g[1, 0] > V[2, shape(V)[1] - 1]:
                    if q_g[0, 0] < V[1, shape(V)[1] - 1]:  # second quadrant
                        theta_orientation = pi - theta_orientation
                    elif q_g[0, 0] > V[1, shape(V)[1] - 1]:  # first quadrant
                        theta_orientation = theta_orientation
                elif q_g[1, 0] < V[2, shape(V)[1] - 1]:
                    if q_g[0, 0] < V[1, shape(V)[1] - 1]:  #third quadrant
                        theta_orientation = pi + theta_orientation
                    elif q_g[0, 0] > V[1, shape(V)[1] - 1]:  # foruth quadrant
                        theta_orientation = 2 * pi - theta_orientation

            ################################## PRINT PLT #################

            if connect_goal:
                if plotting == True:
                    if operate_system == 1:
                        plt.suptitle('Randomly-exploring rapid tree',
                                     fontsize=12)
                        BoundPolyPoints = asarray(
                            PolyUtils.pointList(BoundPoly))
                        plt.plot(BoundPolyPoints[:, 0], BoundPolyPoints[:, 1],
                                 'k')
                        plt.xlabel('x')
                        plt.ylabel('y')
                        if shape(V)[1] <= 2:
                            plt.plot((V[1, shape(V)[1] - 1], q_g[0, 0]),
                                     (V[2, shape(V)[1] - 1], q_g[1, 0]), 'b')
                        else:
                            plt.plot((V[1, E[0, shape(E)[1] - 1]],
                                      V[1, shape(V)[1] - 1], q_g[0, 0]),
                                     (V[2, E[0, shape(E)[1] - 1]],
                                      V[2, shape(V)[1] - 1], q_g[1, 0]), 'b')
                        plt.figure(original_figure).canvas.draw()
                    else:
                        ax.suptitle('Randomly-exploring rapid tree',
                                    fontsize=12)
                        BoundPolyPoints = asarray(
                            PolyUtils.pointList(BoundPoly))
                        ax.plot(BoundPolyPoints[:, 0], BoundPolyPoints[:, 1],
                                'k')
                        ax.xlabel('x')
                        ax.ylabel('y')
                        if shape(V)[1] <= 2:
                            ax.plot((V[1, shape(V)[1] - 1], q_g[0, 0]),
                                    (V[2, shape(V)[1] - 1], q_g[1, 0]), 'b')
                        else:
                            ax.plot((V[1, E[0, shape(E)[1] - 1]],
                                     V[1, shape(V)[1] - 1], q_g[0, 0]),
                                    (V[2, E[0, shape(E)[1] - 1]],
                                     V[2, shape(V)[1] - 1], q_g[1, 0]), 'b')

            if connect_goal and abs(theta_orientation - thetaPrev) < max_angle:
                #if connect_goal and abs(theta_orientation - thetaPrev) < pi/3:
                print "connection is true.Path = 1"
                path = 1
                q_pass = hstack((q_pass, vstack((i, q_g))))
                q_pass_dist = hstack((q_pass_dist, dist))

            i = i + 1

        # connection to goal has established
        if path == 1:
            if shape(q_pass_dist)[0] == 1:
                cols = 0
            else:
                (cols, ) = nonzero(q_pass_dist == min(q_pass_dist))
                cols = asarray(cols)[0]
            q_g = q_pass[1:, cols]  ###Catherine
            q_g = q_g - (q_gBundle[:, q_pass[0, cols]] - V[1:,
                                                           (shape(V)[1] - 1)]
                         ) / norm(q_gBundle[:, q_pass[0, cols]] -
                                  V[1:,
                                    (shape(V)[1] - 1)]) * 3 * radius  #org 3
            if not nextRegionPoly.isInside(q_g[0], q_g[1]):
                q_g = q_g + (q_gBundle[:, q_pass[0, cols]] -
                             V[1:, (shape(V)[1] - 1)]) / norm(
                                 q_gBundle[:, q_pass[0, cols]] -
                                 V[1:, (shape(V)[1] - 1)]) * 6 * radius  #org 3

            if plotting == True:
                if operate_system == 1:
                    plt.plot(q_g[0, 0], q_g[1, 0], 'ko')
                    plt.figure(original_figure).canvas.draw()
                else:
                    ax.plot(q_g[0, 0], q_g[1, 0], 'ko')

            numOfPoint = floor(norm(V[1:, shape(V)[1] - 1] - q_g) / step_size)
            if numOfPoint < 3:
                numOfPoint = 3
            x = linspace(V[1, shape(V)[1] - 1], q_g[0, 0], numOfPoint)
            y = linspace(V[2, shape(V)[1] - 1], q_g[1, 0], numOfPoint)
            for i in range(x.shape[0]):
                if i != 0:
                    V = hstack((V, vstack((shape(V)[1], x[i], y[i]))))
                    E = hstack((E, vstack((shape(V)[1] - 2, shape(V)[1] - 1))))

        # path is not formed, try to append points onto the tree
        if path == 0:
            success = 0  # whether adding a new point is successful
            hit_count = 0  # count for regenerating new edge with the same q_rand
            Icurrent = [
            ]  # to keep track of the index of the closest point to q_n

            while success == 0 and hit_count <= 2:
                if stuck > stuck_thres:
                    omega = random.choice(omega_range_abso)
                else:
                    #!!!! CONTROL SPACE STEP 1 - generate random omega
                    omega = random.choice(omega_range)

                #!!!! CONTROL SPACE STEP 2 - pick a random point on the tree
                tree_index = random.choice(array(V[0])[0])
                xPrev = V[1, tree_index]
                yPrev = V[2, tree_index]
                thetaPrev = V_theta[tree_index]

                j = 1
                #!!!! CONTROL SPACE STEP 3 - Check path of the robot
                path_robot = PolyShapes.Circle(radius, (xPrev, yPrev))
                while j <= timeStep:
                    xOrg = xPrev
                    yOrg = yPrev
                    xPrev = xPrev + velocity / omega * (
                        sin(omega * 1 + thetaPrev) - sin(thetaPrev))
                    yPrev = yPrev - velocity / omega * (
                        cos(omega * 1 + thetaPrev) - cos(thetaPrev))
                    thetaPrev = omega * 1 + thetaPrev
                    path_robot = path_robot + PolyShapes.Circle(
                        radius, (xPrev, yPrev))

                    j = j + 1

                path_all = PolyUtils.convexHull(path_robot)
                in_bound = BoundPoly.covers(path_all)

                # plotting
                plotPoly(path_all, 'r', 1)
                plotMap(original_figure, BoundPoly, allRegions)

                stuck = stuck + 1
                if in_bound:
                    stuck = stuck - 5
                    x = []
                    y = []
                    for k in PolyUtils.pointList(path_all):
                        x = hstack((x, k[0]))
                        y = hstack((y, k[1]))

                    V = hstack((V, vstack((shape(V)[1], xPrev, yPrev))))
                    V_theta = hstack((V_theta, thetaPrev))
                    E = hstack((E, vstack((tree_index, shape(V)[1] - 1))))
                    Other = hstack((Other, vstack((velocity, omega))))
                    ##################### E should add omega and velocity
                    success = 1

    if finish_print == 1:
        print 'Here is the V matrix:', V, 'Here is the E matrix:', E
        print >> sys.__stdout__, 'Here is the V matrix:\n', V, '\nHere is the E matrix:\n', E

    #B: trim to a single path
    single = 0
    while single == 0:
        trim = 0
        for j in range(shape(V)[1] - 3):
            (row, col) = nonzero(E == j + 1)
            if len(col) == 1:
                E = delete(E, col[0], 1)
                trim = 1

        if trim == 0:
            single = 1

    # generate V to be sent to SimGUI to plot
    V_toPass = V[0:, 0]
    E_toPass = [[], []]
    for i in range(shape(E)[1]):
        V_toPass = hstack(
            (V_toPass, vstack((i, V[1, E[1, i - 1]], V[2, E[1, i - 1]]))))
        E_toPass = hstack((E_toPass, vstack((i - 1, i))))

    ####print with matlib
    if plotting == True:
        if operate_system == 1:
            BoundPolyPoints = asarray(PolyUtils.pointList(BoundPoly))
            plt.plot(BoundPolyPoints[:, 0], BoundPolyPoints[:, 1], 'k')
            plt.plot(V[1, :], V[2, :], 'b')
            for i in range(shape(E)[1] - 1):
                plt.text(V[1, E[0, i]],
                         V[2, E[0, i]],
                         V[0, E[0, i]],
                         fontsize=12)
                plt.text(V[1, E[1, i]],
                         V[2, E[1, i]],
                         V[0, E[1, i]],
                         fontsize=12)
            plt.figure(original_figure).canvas.draw()
        else:
            BoundPolyPoints = asarray(PolyUtils.pointList(BoundPoly))
            ax.plot(BoundPolyPoints[:, 0], BoundPolyPoints[:, 1], 'k')
            ax.plot(V[1, :], V[2, :], 'b')
            for i in range(shape(E)[1] - 1):
                ax.text(V[1, E[0, i]],
                        V[2, E[0, i]],
                        V[0, E[0, i]],
                        fontsize=12)
                ax.text(V[1, E[1, i]],
                        V[2, E[1, i]],
                        V[0, E[1, i]],
                        fontsize=12)

    heading = E[0, 0]
    # parse string for RRT printing in GUI (in format: RRT:E[[1,2,3]]:V[[1,2,3]])
    V = array(V)
    V_toPass = array(V_toPass)
    E_toPass = array(E_toPass)
    return V, E, heading, 0, V_toPass, E_toPass
Esempio n. 6
0
    def generateNewNode(self,V,V_theta,E,Other,regionPoly,stuck,append_after_latest_node =False):
        """
        Generate a new node on the current tree matrix
        V         : the node matrix
        V_theta   : the orientation matrix
        E         : the tree matrix (or edge matrix)
        Other     : the matrix containing the velocity and angular velocity(omega) information
        regionPoly: the polygon of current region
        stuck     : count on the number of times failed to generate new node
        append_after_latest_node : append new nodes to the latest node (True only if the previous node addition is successful)
        """


        if self.system_print == True:
            print "In control space generating path,stuck = " + str(stuck)

        connection_to_tree = False   # True when connection to the tree is successful

        if stuck > self.stuck_thres:
            # increase the range of omega since path cannot ge generated
            omega = random.choice(self.omega_range_escape)
        else:
            #!!!! CONTROL SPACE STEP 1 - generate random omega
            omega = random.choice(self.omega_range)


        #!!!! CONTROL SPACE STEP 2 - pick a random point on the tree
        if append_after_latest_node:
            tree_index = shape(V)[1]-1
        else:
            if random.choice([1,2]) == 1:
                tree_index = random.choice(array(V[0])[0])
            else:
                tree_index = shape(V)[1]-1


        xPrev     = V[1,tree_index]
        yPrev     = V[2,tree_index]
        thetaPrev = V_theta[tree_index]

        j = 1
        #!!!! CONTROL SPACE STEP 3 - Check path of the robot
        path_robot = PolyShapes.Circle(self.radius,(xPrev,yPrev))
        while j <= self.timeStep:
            xOrg      = xPrev
            yOrg      = yPrev
            xPrev     = xPrev + self.velocity/omega*(sin(omega* 1 + thetaPrev)-sin(thetaPrev))
            yPrev     = yPrev - self.velocity/omega*(cos(omega* 1 + thetaPrev)-cos(thetaPrev))
            thetaPrev = omega* 1 + thetaPrev
            path_robot = path_robot + PolyShapes.Circle(self.radius,(xPrev,yPrev))

            j = j + 1

        thetaPrev = self.orientation_bound(thetaPrev)
        path_all = PolyUtils.convexHull(path_robot)
        in_bound = regionPoly.covers(path_all)

        # plotting
        if self.plotting == True:
            self.plotPoly(path_all,'r',1)

        stuck = stuck + 1

        if in_bound:
            robot_new_node = PolyShapes.Circle(self.radius,(xPrev,yPrev))
            # check how many nodes on the tree does the new node overlaps with
            nodes_overlap_count = 0
            for k in range(shape(V)[1]-1):
                robot_old_node = PolyShapes.Circle(self.radius,(V[1,k],V[2,k]))
                if robot_new_node.overlaps(robot_old_node):
                    if  abs(thetaPrev - V_theta[k]) <   self.max_angle_overlap:
                        nodes_overlap_count += 1


            if nodes_overlap_count == 0 or (stuck > self.stuck_thres+1 and nodes_overlap_count < 2) or (stuck > self.stuck_thres+500):
                if  stuck > self.stuck_thres+1:
                    append_after_latest_node  = False

                if (stuck > self.stuck_thres+500):
                    stuck = 0
                stuck = stuck - 20
                # plotting
                if self.plotting == True:
                    self.plotPoly(path_all,'b',1)

                if self.system_print == True:
                    print "node connected"

                V = hstack((V,vstack((shape(V)[1],xPrev,yPrev))))
                V_theta = hstack((V_theta,thetaPrev))
                E = hstack((E,vstack((tree_index ,shape(V)[1]-1))))
                Other = hstack((Other,vstack((self.velocity,omega))))
                ##################### E should add omega and velocity
                connection_to_tree = True
                append_after_latest_node  = True
            else:
                append_after_latest_node = False

                if self.system_print == True:
                    print "node not connected. check goal point"

        else:
            append_after_latest_node = False

        return  V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree
Esempio n. 7
0
    def buildTree(self,p,theta,regionPoly,nextRegionPoly,q_gBundle,face_normal, last=False):
        """
        This function builds the RRT tree.
        p                : x,y position of the robot
        theta            : current orientation of the robot
        regionPoly       : current region polygon
        nextRegionPoly   : next region polygon
        q_gBundle        : coordinates of q_goals that the robot can reach
        face_normal      : the normal vector of each face corresponding to each goal point in q_gBundle
        """

        q_init          = mat(p).T
        V               = vstack((0,q_init))
        theta           = self.orientation_bound(theta)
        V_theta         = array([theta])

        #!!! CONTROL SPACE: generate a list of omega for random sampling
        omegaLowerBound = -math.pi/20      # upper bound for the value of omega
        omegaUpperBound = math.pi/20       # lower bound for the value of omega
        omegaNoOfSteps  = 20
        self.omega_range = linspace(omegaLowerBound,omegaUpperBound,omegaNoOfSteps)
        self.omega_range_escape = linspace(omegaLowerBound*4,omegaUpperBound*4,omegaNoOfSteps*4)    # range used when stuck > stuck_thres

        regionPolyOld = Polygon.Polygon(regionPoly)
        #regionPoly += PolyShapes.Circle(self.radius*2.5,(q_init[0,0],q_init[1,0]))
        regionPoly += PolyShapes.Circle(self.radius*4,(q_init[0,0],q_init[1,0]))

        # check faces of the current region for goal points
        E     = [[],[]]       # the tree matrix
        Other = [[],[]]
        path                        = False          # if path formed then = 1
        stuck                       = 0              # count for changing the range of sampling omega
        append_after_latest_node    = False       # append new nodes to the latest node

        if self.system_print == True:
            print "plotting in buildTree is " + str(self.plotting)
        if self.plotting == True:
            if not plt.isinteractive():
                plt.ion()
            plt.hold(True)


        while not path:
            #step -1: try connection to q_goal (generate path to goal)
            i = 0
            ltlmop_logger.debug("finding RRT Path...")
            if self.system_print == True:
                print "Try Connection to the goal points"

            # pushing possible q_goals into the current region (ensure path is covered by the current region polygon)
            q_pass = [[],[],[]]
            q_pass_dist = []
            q_gBundle = mat(q_gBundle)
            face_normal = mat(face_normal)

            while i < q_gBundle.shape[1]:
                q_g_original = q_gBundle[:,i]
                q_g = q_gBundle[:,i]+face_normal[:,i]*1.5*self.radius    ##original 2*self.radius
                #q_g = q_gBundle[:,i]+(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*1.5*self.radius    ##original 2*self.radius
                if not regionPolyOld.isInside(q_g[0],q_g[1]):
                    #q_g = q_gBundle[:,i]-(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*1.5*self.radius    ##original 2*self.radius
                    q_g = q_gBundle[:,i]-face_normal[:,i]*1.5*self.radius    ##original 2*self.radius

                #forming polygon for path checking
                EdgePolyGoal = PolyShapes.Circle(self.radius,(q_g[0,0],q_g[1,0])) + PolyShapes.Circle(self.radius,(V[1,shape(V)[1]-1],V[2:,shape(V)[1]-1]))
                EdgePolyGoal = PolyUtils.convexHull(EdgePolyGoal)
                dist = norm(q_g - V[1:,shape(V)[1]-1])

                #check connection to goal
                connect_goal = regionPoly.covers(EdgePolyGoal)   #check coverage of path from new point to goal

                # compare orientation difference
                thetaPrev = V_theta[shape(V)[1]-1]

                if not (q_g[0,0]- V[1,shape(V)[1]-1]) == 0:
                    theta_orientation = abs(arctan((q_g[1,0]- V[2,shape(V)[1]-1])/(q_g[0,0]- V[1,shape(V)[1]-1])))
                else:
                    theta_orientation = 0
                if q_g[1,0] > V[2,shape(V)[1]-1]:
                    if q_g[0,0] < V[1,shape(V)[1]-1]: # second quadrant
                        theta_orientation = pi - theta_orientation
                    elif q_g[0,0] > V[1,shape(V)[1]-1]: # first quadrant
                        theta_orientation = theta_orientation
                elif q_g[1,0] < V[2,shape(V)[1]-1]:
                    if q_g[0,0] < V[1,shape(V)[1]-1]: #third quadrant
                        theta_orientation = pi + theta_orientation
                    elif q_g[0,0] > V[1,shape(V)[1]-1]: # foruth quadrant
                        theta_orientation =  2*pi - theta_orientation

                # check the angle between vector(new goal to goal_original ) and vector( latest node to new goal)
                Goal_to_GoalOriginal = q_g_original - q_g
                LatestNode_to_Goal   = q_g - V[1:,shape(V)[1]-1]
                Angle_Goal_LatestNode= arccos(vdot(array(Goal_to_GoalOriginal), array(LatestNode_to_Goal))/norm(Goal_to_GoalOriginal)/norm(LatestNode_to_Goal))

                # if connection to goal can be established and the max change in orientation of the robot is smaller than max_angle, tree is said to be completed.
                if self.orientation_print == True:
                    print "theta_orientation is " + str(theta_orientation)
                    print "thetaPrev is " + str(thetaPrev)
                    print "(theta_orientation - thetaPrev) is " + str(abs(theta_orientation - thetaPrev))
                    print "self.max_angle_allowed is " + str(self.max_angle_allowed)
                    print "abs(theta_orientation - thetaPrev) < self.max_angle_allowed" + str(abs(theta_orientation - thetaPrev) < self.max_angle_allowed)
                    print"Goal_to_GoalOriginal: " + str( array(Goal_to_GoalOriginal)) + "; LatestNode_to_Goal: " + str( array(LatestNode_to_Goal))
                    print vdot(array(Goal_to_GoalOriginal), array(LatestNode_to_Goal))
                    print "Angle_Goal_LatestNode" + str(Angle_Goal_LatestNode)

                if connect_goal and (abs(theta_orientation - thetaPrev) < self.max_angle_allowed) and (Angle_Goal_LatestNode < self.max_angle_allowed):

                    path = True
                    q_pass = hstack((q_pass,vstack((i,q_g))))
                    q_pass_dist = hstack((q_pass_dist,dist))

                i = i + 1

            if self.system_print == True:
                print "checked goal points"

            self.E = E
            self.V = V
            # connection to goal has established
            # Obtain the closest goal point that path can be formed.
            if path:
                if shape(q_pass_dist)[0] == 1:
                    cols = 0
                else:
                    (cols,) = nonzero(q_pass_dist == min(q_pass_dist))
                    cols = asarray(cols)[0]
                q_g = q_pass[1:,cols]
                """
                q_g = q_g-(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*3*self.radius   #org 3
                if not nextRegionPoly.isInside(q_g[0],q_g[1]):
                    q_g = q_g+(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*6*self.radius   #org 3
                """
                if self.plotting == True :
                    if self.operate_system == 1:
                        plt.suptitle('Rapidly-exploring Random Tree', fontsize=12)
                        plt.xlabel('x')
                        plt.ylabel('y')
                        if shape(V)[1] <= 2:
                            plt.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        else:
                            plt.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        plt.plot(q_g[0,0],q_g[1,0],'ko')
                        plt.figure(1).canvas.draw()
                        plt.pause(0.0001)
                    else:
                        BoundPolyPoints = asarray(PolyUtils.pointList(regionPoly))
                        self.ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
                        if shape(V)[1] <= 2:
                            self.ax.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        else:
                            self.ax.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        self.ax.plot(q_g[0,0],q_g[1,0],'ko')

                # trim the path connecting current node to goal point into pieces if the path is too long now
                numOfPoint = floor(norm(V[1:,shape(V)[1]-1]- q_g)/self.step_size)
                if numOfPoint < 3:
                    numOfPoint = 3
                x = linspace( V[1,shape(V)[1]-1], q_g[0,0], numOfPoint )
                y = linspace( V[2,shape(V)[1]-1], q_g[1,0], numOfPoint )
                for i in range(x.shape[0]):
                    if i != 0:
                        V = hstack((V,vstack((shape(V)[1],x[i],y[i]))))
                        E = hstack((E,vstack((shape(V)[1]-2,shape(V)[1]-1))))

                #push the goal point to the next region
                if self.system == 1:
                    q_g = q_g+face_normal[:,q_pass[0,cols]]*4*self.radius    ##original 3*self.radius
                else:
                    q_g = q_g+face_normal[:,q_pass[0,cols]]*3*self.radius    ##original 3*self.radius

                if not nextRegionPoly.isInside(q_g[0],q_g[1]):
                    if self.system == 1:
                        q_g = q_g-face_normal[:,q_pass[0,cols]]*8*self.radius    ##original 6*self.radius
                    else:
                        q_g = q_g-face_normal[:,q_pass[0,cols]]*6*self.radius    ##original 6*self.radius

                V = hstack((V,vstack((shape(V)[1],q_g[0,0],q_g[1,0]))))
                E = hstack((E,vstack((shape(V)[1]-2 ,shape(V)[1]-1))))

                if self.plotting == True :
                    if self.operate_system == 1:
                        plt.plot(q_g[0,0],q_g[1,0],'ko')
                        plt.plot(( V[1,shape(V)[1]-1],V[1,shape(V)[1]-2]),( V[2,shape(V)[1]-1],V[2,shape(V)[1]-2]),'b')
                        plt.figure(1).canvas.draw()
                        plt.pause(0.0001)
                    else:
                        self.ax.plot(q_g[0,0],q_g[1,0],'ko')
                        self.ax.plot(( V[1,shape(V)[1]-1],V[1,shape(V)[1]-2]),( V[2,shape(V)[1]-1],V[2,shape(V)[1]-2]),'b')

            # path is not formed, try to append points onto the tree
            if not path:

                # connection_to_tree : connection to the tree is successful
                if append_after_latest_node:
                    V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree = self.generateNewNode(V,V_theta,E,Other,regionPoly,stuck, append_after_latest_node)
                else:
                    connection_to_tree = False
                    while not connection_to_tree:
                        V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree = self.generateNewNode  (V,V_theta,E,Other,regionPoly,stuck)


        if self.finish_print:
            print 'Here is the V matrix:', V, 'Here is the E matrix:',E
            print >>sys.__stdout__, 'Here is the V matrix:\n', V, '\nHere is the E matrix:\n',E

        #B: trim to a single path
        single = 0
        while single == 0:
            trim  = 0
            for j in range(shape(V)[1]-3):
                (row,col) = nonzero(E == j+1)
                if len(col) == 1:
                    E = delete(E, col[0], 1)
                    trim = 1

            if trim == 0:
                single = 1;

        ####print with matlib
        if self.plotting ==True :
            if self.operate_system == 1:
                plt.plot(V[1,:],V[2,:],'b')
                for i in range(shape(E)[1]):
                    plt.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
                    plt.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)
                plt.figure(1).canvas.draw()
                plt.pause(0.0001)

            else:
                BoundPolyPoints = asarray(PolyUtils.pointList(regionPoly))
                self.ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
                self.ax.plot(V[1,:],V[2,:],'b')
                for i in range(shape(E)[1]):
                    self.ax.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
                    self.ax.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)

        # plot in simGUI
        self.path = []
        for i in range(shape(E)[1]):
            self.path.append(tuple(map(int, self.executor.hsub.coordmap_lab2map([V[1,E[0,i]],V[2,E[0,i]]]))))
        self.path.append(tuple(map(int, self.executor.hsub.coordmap_lab2map([V[1,E[1,i]],V[2,E[1,i]]]))))
        self.executor.postEvent("RRT", self.path)

        #return V, E, and the current node number on the tree
        V = array(V)
        return V, E, 0
Esempio n. 8
0
def buildTree(p,theta,vert, R, system, regionPoly,nextRegionPoly,q_gBundle,\
mappedRegions,allRegions,max_angle_allowed, plotting, operate_system,ax, last=False):

    """
    This function builds the RRT tree
    pose: x,y position of the robot
    theta: current orientation of the robot
    R: radius of the robot
    system: determine step_size and radius for the example
    regionPoly: current region polygon
    nextRegionPoly: next region polygon
    q_gBundle: coordinates of q_goals that the robot can reach
    mappedRegions: region polygons
    allRegions: polygon that includes all the region
    max_angle_allowed: the difference in angle between the two nodes allowed (between 0 to 2*pi)
    plotting: 1 if plotting is enabled, 0- disabled
    operate_system: Which operating system is used for execution. Ubuntu and Mac is 1, Windows is 2
    ax: plot for windows
    """

    max_angle = max_angle_allowed
    """
    if operate_system == 2:
        # start using anmination to plot Pioneer
        self.fig = plt.figure()
        self.ax = self.fig.add_subplot(111)
        self.scope = _Scope(self.ax,self)
        thread.start_new_thread(self.jplot,())
    """

    ## 1: Nao ; 2: STAGE ;  3: ODE
    if system == 1:    ## Nao
        step_size  = 0.2          	#set the step_size for points be 1/5 of the norm from   ORIGINAL = 0.4
    elif system == 2:
        step_size  = 0.5
    elif system == 3:
        step_size  = 15

    if system == 1:    ## Nao
        timeStep = 5   #time step for calculation of x, y position
    elif system == 2:
        timeStep = 4   #time step for calculation of x, y position
    elif system == 3:
        timeStep = 10  #time step for calculation of x, y position    #10
    #############tune velocity OMEGA, TIME STEP

    #fix velocity
    ## 1: Nao ; 2: STAGE ;  3: ODE
    if system == 1:    ## Nao
        velocity  = 0.05          	#set the step_size for points be 1/5 of the norm from
    elif system == 2:
        velocity  = 0.06
    elif system == 3:
        velocity = 2    # what is used in RRTControllerHelper.setVelocity    #1.5
    #############tune velocity OMEGA, TIME STEP

    BoundPoly  = regionPoly       # Boundary polygon = current region polygon
    radius     = R
    q_init     = mat(p).T
    V          = vstack((0,q_init))
    V_theta    = array([theta])
    original_figure = 1

    #!!! CONTROL SPACE: generate a list of omega for random sampling
    omegaLowerBound = -math.pi/20      # upper bound for the value of omega
    omegaUpperBound = math.pi/20       # lower bound for the value of omega
    omegaStepSize   = 20
    omega_range = linspace(omegaLowerBound,omegaUpperBound,omegaStepSize)
    omega_range_abso = linspace(omegaLowerBound*4,omegaUpperBound*4,omegaStepSize*4)    # range used when stuck > stuck_thres
    edgeX    = []
    edgeY    = []

    # check faces of the current region for goal points

    E = [[],[]]
    Other = [[],[]]
    path     = 0          # if path formed then = 1
    stuck    = 0          # count for changing the range of sampling omega
    stuck_thres = 300     # threshold for changing the range of sampling omega

    if not plt.isinteractive():
        plt.ion()
    plt.hold(True)
    while path == 0:
        #step -1: try connection to q_goal (generate path to goal)
        goalCheck = 0;
        i = 0

        # pushing possible q_goals into the current region (ensure path is covered by the current region polygon)
        q_pass = [[],[],[]]
        q_pass_dist = []
        q_gBundle = mat(q_gBundle)
        while i < q_gBundle.shape[1]:   ###not sure about shape
            q_g = q_gBundle[:,i]+(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*radius    ##original 2*radius
            trial = 1
            if not BoundPoly.isInside(q_g[0],q_g[1]):
                trial = 2
                q_g = q_gBundle[:,i]-(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*radius    ##original 2*radius

            #forming polygon for path checking
            cross_goal     = cross(vstack((q_g-vstack((V[1,shape(V)[1]-1],V[2,shape(V)[1]-1])),0)).T,hstack((0,0,1)))
            cross_goal       = cross_goal.T
            move_vector_goal = radius*cross_goal[0:2]/sqrt((cross_goal[0,0]**2 + cross_goal[1,0]**2))
            upperEdgeG   = hstack((vstack((V[1,shape(V)[1]-1],V[2,shape(V)[1]-1])),q_g)) + hstack((move_vector_goal,move_vector_goal))
            lowerEdgeG   = hstack((vstack((V[1,shape(V)[1]-1],V[2,shape(V)[1]-1])),q_g)) - hstack((move_vector_goal,move_vector_goal))
            EdgePolyGoal    = Polygon.Polygon((tuple(array(lowerEdgeG[:,0].T)[0]),tuple(array(upperEdgeG[:,0].T)[0]),tuple(array(upperEdgeG[:,1].T)[0]),tuple(array(lowerEdgeG[:,1].T)[0])))

            dist = norm(q_g - V[1:,shape(V)[1]-1])
            connect_goal = BoundPoly.covers(EdgePolyGoal)   #check coverage of path from new point to goal
            #check connection to goal

            """
            if connect_goal:
                print "connection is true"
                path = 1
                q_pass = hstack((q_pass,vstack((i,q_g))))
                q_pass_dist = hstack((q_pass_dist,dist))
            """

            # compare orientation difference
            thetaPrev = V_theta[shape(V)[1]-1]
            theta_orientation = abs(arctan((q_g[1,0]- V[2,shape(V)[1]-1])/(q_g[0,0]- V[1,shape(V)[1]-1])))
            if thetaPrev < 0:
                if q_g[1,0] > V[2,shape(V)[1]-1]:
                    if q_g[0,0] < V[1,shape(V)[1]-1]: # second quadrant
                        theta_orientation = -2*pi + theta_orientation
                    elif q_g[0,0] > V[1,shape(V)[1]-1]: # first quadrant
                        theta_orientation = -pi -theta_orientation
                elif q_g[1,0] < V[2,shape(V)[1]-1]:
                    if q_g[0,0] < V[1,shape(V)[1]-1]: #third quadrant
                        theta_orientation = -pi + theta_orientation
                    elif q_g[0,0] > V[1,shape(V)[1]-1]: # foruth quadrant
                        theta_orientation =  - theta_orientation
            else:
                if q_g[1,0] > V[2,shape(V)[1]-1]:
                    if q_g[0,0] < V[1,shape(V)[1]-1]: # second quadrant
                        theta_orientation = pi - theta_orientation
                    elif q_g[0,0] > V[1,shape(V)[1]-1]: # first quadrant
                        theta_orientation = theta_orientation
                elif q_g[1,0] < V[2,shape(V)[1]-1]:
                    if q_g[0,0] < V[1,shape(V)[1]-1]: #third quadrant
                        theta_orientation = pi + theta_orientation
                    elif q_g[0,0] > V[1,shape(V)[1]-1]: # foruth quadrant
                        theta_orientation =  2*pi - theta_orientation

            ################################## PRINT PLT #################

            if connect_goal :
                if plotting == True:
                    if operate_system == 1:
                        plt.suptitle('Randomly-exploring rapid tree', fontsize=12)
                        BoundPolyPoints = asarray(PolyUtils.pointList(BoundPoly))
                        plt.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
                        plt.xlabel('x')
                        plt.ylabel('y')
                        if shape(V)[1] <= 2:
                            plt.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        else:
                            plt.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        plt.figure(original_figure).canvas.draw()
                    else:
                        ax.suptitle('Randomly-exploring rapid tree', fontsize=12)
                        BoundPolyPoints = asarray(PolyUtils.pointList(BoundPoly))
                        ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
                        ax.xlabel('x')
                        ax.ylabel('y')
                        if shape(V)[1] <= 2:
                            ax.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        else:
                            ax.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')


            if connect_goal and abs(theta_orientation - thetaPrev) < max_angle:
                #if connect_goal and abs(theta_orientation - thetaPrev) < pi/3:
                print "connection is true.Path = 1"
                path = 1
                q_pass = hstack((q_pass,vstack((i,q_g))))
                q_pass_dist = hstack((q_pass_dist,dist))


            i = i + 1

        # connection to goal has established
        if path == 1:
            if shape(q_pass_dist)[0] == 1:
                cols = 0
            else:
                (cols,) = nonzero(q_pass_dist == min(q_pass_dist))
                cols = asarray(cols)[0]
            q_g = q_pass[1:,cols]   ###Catherine
            q_g = q_g-(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*3*radius   #org 3
            if not nextRegionPoly.isInside(q_g[0],q_g[1]):
                q_g = q_g+(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*6*radius   #org 3

            if plotting == True:
                if operate_system == 1:
                    plt.plot(q_g[0,0],q_g[1,0],'ko')
                    plt.figure(original_figure).canvas.draw()
                else:
                    ax.plot(q_g[0,0],q_g[1,0],'ko')


            numOfPoint = floor(norm(V[1:,shape(V)[1]-1]- q_g)/step_size)
            if numOfPoint < 3:
                numOfPoint = 3
            x = linspace( V[1,shape(V)[1]-1], q_g[0,0], numOfPoint )
            y = linspace( V[2,shape(V)[1]-1], q_g[1,0], numOfPoint )
            for i in range(x.shape[0]):
                if i != 0:
                    V = hstack((V,vstack((shape(V)[1],x[i],y[i]))))
                    E = hstack((E,vstack((shape(V)[1]-2,shape(V)[1]-1))))

        # path is not formed, try to append points onto the tree
        if path == 0:
            success     = 0           # whether adding a new point is successful
            hit_count   = 0           # count for regenerating new edge with the same q_rand
            Icurrent    = []          # to keep track of the index of the closest point to q_n

            while success == 0 and hit_count <= 2:
                if stuck > stuck_thres:
                    omega = random.choice(omega_range_abso)
                else:
                    #!!!! CONTROL SPACE STEP 1 - generate random omega
                    omega = random.choice(omega_range)


                #!!!! CONTROL SPACE STEP 2 - pick a random point on the tree
                tree_index = random.choice(array(V[0])[0])
                xPrev     = V[1,tree_index]
                yPrev     = V[2,tree_index]
                thetaPrev = V_theta[tree_index]

                j = 1
                #!!!! CONTROL SPACE STEP 3 - Check path of the robot
                path_robot = PolyShapes.Circle(radius,(xPrev,yPrev))
                while j <= timeStep:
                    xOrg      = xPrev
                    yOrg      = yPrev
                    xPrev     = xPrev + velocity/omega*(sin(omega* 1 + thetaPrev)-sin(thetaPrev))
                    yPrev     = yPrev - velocity/omega*(cos(omega* 1 + thetaPrev)-cos(thetaPrev))
                    thetaPrev = omega* 1 + thetaPrev
                    path_robot = path_robot + PolyShapes.Circle(radius,(xPrev,yPrev))

                    j = j + 1

                path_all = PolyUtils.convexHull(path_robot)
                in_bound = BoundPoly.covers(path_all)

                # plotting
                plotPoly(path_all,'r',1)
                plotMap(original_figure,BoundPoly,allRegions)

                stuck = stuck + 1
                if in_bound:
                    stuck = stuck -5
                    x = []
                    y = []
                    for k in  PolyUtils.pointList(path_all):
                        x = hstack((x,k[0]))
                        y = hstack((y,k[1]))

                    V = hstack((V,vstack((shape(V)[1],xPrev,yPrev))))
                    V_theta = hstack((V_theta,thetaPrev))
                    E = hstack((E,vstack((tree_index ,shape(V)[1]-1))))
                    Other = hstack((Other,vstack((velocity,omega))))
                    ##################### E should add omega and velocity
                    success = 1

    if finish_print == 1:
        print 'Here is the V matrix:', V, 'Here is the E matrix:',E
        print >>sys.__stdout__, 'Here is the V matrix:\n', V, '\nHere is the E matrix:\n',E

    #B: trim to a single path
    single = 0
    while single == 0:
        trim  = 0
        for j in range(shape(V)[1]-3):
            (row,col) = nonzero(E == j+1)
            if len(col) == 1:
                E = delete(E, col[0], 1)
                trim = 1

        if trim == 0:
            single = 1;

    # generate V to be sent to SimGUI to plot
    V_toPass = V[0:,0]
    E_toPass = [[],[]]
    for i in range(shape(E)[1]):
        V_toPass = hstack((V_toPass,vstack((i,V[1,E[1,i-1]],V[2,E[1,i-1]]))))
        E_toPass = hstack((E_toPass,vstack((i-1,i))))

    ####print with matlib
    if plotting ==True:
        if operate_system == 1:
            BoundPolyPoints = asarray(PolyUtils.pointList(BoundPoly))
            plt.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
            plt.plot(V[1,:],V[2,:],'b')
            for i in range(shape(E)[1]-1):
                plt.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
                plt.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)
            plt.figure(original_figure).canvas.draw()
        else:
            BoundPolyPoints = asarray(PolyUtils.pointList(BoundPoly))
            ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
            ax.plot(V[1,:],V[2,:],'b')
            for i in range(shape(E)[1]-1):
                ax.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
                ax.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)

    heading  = E[0,0]
    # parse string for RRT printing in GUI (in format: RRT:E[[1,2,3]]:V[[1,2,3]])
    V = array(V)
    V_toPass = array(V_toPass)
    E_toPass = array(E_toPass)
    return V, E, heading,0,V_toPass,E_toPass