예제 #1
0
def run(obstacles, start, goal, max_size, plotter):
    step_size   = 50
    final_pos   = np.array(goal[:2])
    
    # Pre-compute for generating new random points in Q_free
    minp, maxp   = obstacles.vertices.min(0), obstacles.vertices.max(0)
    span, offset = (maxp-minp)*1.1, minp-(maxp-minp)*0.05

    def gen_valid_rand(valid_function):
        """ Generates a valid q_rand in Q_free given a validity function """
        tmp     = np.random.random(2) * span + offset
        while not valid_function(*tmp):
            tmp = np.random.random(2) * span + offset
        return tmp

    KD    = KDTree(start[:2], 0)
    circ1 = plotter.draw_circle(start, 1, time=1, zorder=5)
    obstacles = Obstacles(obstacles.to_polygons())

    trials = 0
    while KD.length < max_size:
        trials += 1
        circ1.remove()
        # Select a random point q_rand \in Q_free
        q_rand = gen_valid_rand(obstacles.point_is_valid) if np.random.randint(0, 100)>5 else final_pos
        circ1 = plotter.draw_circle(q_rand, 5, time=0.01, zorder=5)

        # Find the nearest node and distance to it
        q_near, dist = KD.nearestNode(q_rand, return_node=True)

        # Generate the next node in the direction of q_rand
        if dist < 0.5: continue
        if dist < step_size:
            if trials < 10: continue
            q_next = tuple(q_rand)
        else:
            q_next = gen_next(tuple(q_near.node), q_rand, step_size)
            if not obstacles.point_is_valid(*q_next): continue
        
        dist = math.hypot(q_next[0]-q_near[0], q_next[1]-q_near[1])
        # Check validity and update tree
        for i in range(10):
            alpha_new = np.random.random() * 2*math.pi #( + q_near.alpha) - math.pi
            collides = check_collision(obstacles, (*q_near.node, q_near.alpha), (*q_next, alpha_new), dist)
            if not collides: break
        else: continue
        
        KD.addNode(q_next, alpha_new)
        plot_steps((*q_near.node, q_near.alpha), (*q_next, alpha_new), dist, plotter)

        goal_distance = math.hypot(q_next[0]-goal[0], q_next[1]-goal[1]) 
        collides = check_collision(obstacles, (*q_next, alpha_new), goal, goal_distance)
        if not collides:
            plot_steps((*q_next, alpha_new), goal, goal_distance, plotter)
            plotter.draw_rectangle(gen_rect_pts(*goal), facecolor='red', edgecolor='k')
            break

        trials = 0

    print("n =", KD.length)
예제 #2
0
def main():
    XDIM = 500
    YDIM = 500
    WINSIZE = (XDIM, YDIM)
    MAX_NUM_NODES = 800
    MIN_NUM_NODES = 400
    pygame.init()
    screen = pygame.display.set_mode(WINSIZE)
    pygame.display.set_caption('RRT* Path Planning')
    screen.fill(WHITE)
    running = True

    # Obstacles
    obs = Obstacles(screen, GRAY)
    obs.make_circle(150, 150, 50)
    obs.make_rect(250, 100, 50, 300)
    obs.draw()

    obs_resolution = 5

    start_point = (50, 50)
    goal_point = (400, 400)
    goal_tolerance = 20

    rrt_star = RRTStar(start_point, goal_point, MAX_NUM_NODES, MIN_NUM_NODES,
                       goal_tolerance, 0, 30, screen, obs, obs_resolution)

    path = rrt_star.planning()
    print "Final Path: "
    print path

    while running:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
예제 #3
0
 def __init__(self, track, riders, players):
     self.track = track
     self.riders = riders
     self.obstacles = Obstacles(riders)
     self.players = players
     self.arrivals = []
     self.checkArrivals()
예제 #4
0
def create_obstacle(settings, obs, grounds, screen):

    random_number2 = randint(settings.ob_number - 2, settings.ob_number)
    for x in range(1, random_number2):

        ob = Obstacles(settings, screen, grounds)
        random_number = randint(0, len(ob.images) - 1)
        ob.index = random_number

        ob.pick_obstacle(grounds)

        if x != 1:
            if x != random_number2:
                random_offset = randint(-20, 20)
            ob.x += settings.previous_position + random_offset
        settings.previous_position += ob.rect.width
        obs.append(ob)
    settings.previous_position = 0
예제 #5
0
 def testObstacles(self):
     riders = [
         RiderToken(0, 0),
         RiderToken(1, 0),
         RiderToken(2, 1),
         RiderToken(3, 0),
         RiderToken(3, 1)
     ]
     obstacles = Obstacles(riders)
     assert_equals([(0, 0), (1, 1), (2, 0), (3, 2), (4, 0)],
                   findPath(obstacles, (0, 0), (4, 0)))
예제 #6
0
def main():
    XDIM = 500
    YDIM = 500
    WINSIZE = [XDIM, YDIM]
    EPSILON = 7.0
    MAX_NUM_NODES = 1000
    MIN_NUM_NODES = 500
    pygame.init()
    screen = pygame.display.set_mode(WINSIZE)
    pygame.display.set_caption('RRT* Dual Tree Path Planning')
    screen.fill(WHITE)

    # Obstacles
    obs = Obstacles(screen, BLACK)
    obs.make_circle(150, 150, 50)
    obs.make_rect(250, 100, 50, 300)
    obs.draw()

    obs_resolution = 5

    start_point = [50, 50]
    goal_point = [400, 400]
    goal_tolerance = 20

    rrt_star = RRTStarDualTree(start_point, goal_point, MAX_NUM_NODES,
                               MIN_NUM_NODES, goal_tolerance, 0, 30, screen,
                               obs, obs_resolution)

    path = rrt_star.planning()
    pause = True
    # for e in pygame.event.get():
    #     if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE):
    #         sys.exit("Leaving because you requested it.")
    # pygame.display.update()

    while pause:
        for event in pygame.event.get():

            if event.type == pygame.QUIT:
                pygame.quit()
                quit()
예제 #7
0
def dwa_wrapper(final_list):

    odom_dec = {}
    q = Quaternion(final_list[0].pose.pose.orientation.w,
                   final_list[0].pose.pose.orientation.x,
                   final_list[0].pose.pose.orientation.y,
                   final_list[0].pose.pose.orientation.z)
    e = q.to_euler(degrees=False)
    odom_dec["x"] = final_list[0].pose.pose.position.x
    odom_dec["y"] = final_list[0].pose.pose.position.y
    odom_dec["theta"] = e[2]
    odom_dec["u"] = final_list[0].twist.twist.linear.x
    odom_dec["omega"] = final_list[0].twist.twist.angular.z

    cnfg = Config(odom_dec, final_list[1])
    obs = Obstacles(final_list[2].ranges, cnfg)
    v_list, w_list, cost_list = DWA(cnfg, obs)

    return v_list, w_list, cost_list, final_list[3]
예제 #8
0
def main():
    XDIM = 500
    YDIM = 500
    WINSIZE = (XDIM, YDIM)
    MAX_NUM_NODES = 800
    MIN_NUM_NODES = 400
    pygame.init()
    # set_mode(size=(0, 0), flags=0, depth=0, display=0, vsync=0)
    screen = pygame.display.set_mode(WINSIZE, pygame.RESIZABLE, 32)
    pygame.display.set_caption('RRT*-Smart Path Planning')
    screen.fill(WHITE)

    # Obstacles
    obs = Obstacles(screen, BLACK)
    obs.make_circle(150, 150, 50)
    obs.make_rect(250, 100, 50, 300)
    obs.draw()

    start_point = (50, 50)
    goal_point = (450, 450)
    goal_tolerance = 20
    obs_resolution = 5
    rrt_star_smart = RRTStarSmart(start_point, goal_point, MAX_NUM_NODES,
                                  MIN_NUM_NODES, goal_tolerance, 0, 30, screen,
                                  obs, obs_resolution)

    path = rrt_star_smart.planning()
    pause = True
    # for e in pygame.event.get():
    #     if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE):
    #         sys.exit("Leaving because you requested it.")
    # pygame.display.update()

    while pause:
        for event in pygame.event.get():

            if event.type == pygame.QUIT:
                pygame.quit()
                quit()
예제 #9
0
파일: game.py 프로젝트: owad/thegame
 def __init__(self):
     self.cfg = config.Config(file('config.ini'))
     self.game_start_time = time.time()
     self.game_speed = self.cfg.init_game_speed
     self.play_time = 0
     self.game_score = 0
     self.player_name = ''
     
     self.fps = self.cfg.fps
     pygame.init()
     try:
         if sys.argv[1] == 'fullscreen':
             info = pygame.display.Info()
             self.window_size = (info.current_w, info.current_h)
             pygame.display.set_mode(self.window_size, pygame.FULLSCREEN)
     except:
         self.window_size = (640, 480)
     self.screen = pygame.display.set_mode(self.window_size)
     self.clock = pygame.time.Clock()
     
     self.obstacles = Obstacles(self.screen, self.window_size)
     self.obstacles.gen_obstacles(1)
     self.player = Player(self.screen, self.window_size)
     self.bg = Background(self.screen)
예제 #10
0
def main():
    #changing the height might mess up the sonar, let me know if it does
    width, height = 1280, 800

    #create game screen & physics
    pygame.init()
    running = True
    screen = pygame.display.set_mode((width, height))
    clock = pygame.time.Clock()
    font = pygame.font.SysFont("Arial", 16)
    space = pymunk.Space()
    space.gravity = 0, 0

    #create player (x pos, y pos, radius)
    player = Player(space, screen, 400, 400, 30)

    #collision handler callback
    def handle(arbiter, space, data):
        player.reset()
        player.collided = True
        return True

    handler = space.add_collision_handler(1, 2)
    handler.begin = handle

    #dont omit
    draw_options = pymunk.pygame_util.DrawOptions(screen)

    #create obstacles
    Obstacles.addWalls(space)
    Obstacles.addObstacles(space)

    #create nn
    nn = NN()

    while running:
        for event in pygame.event.get():
            if event.type == QUIT or \
                event.type == KEYDOWN and (event.key in [K_ESCAPE, K_q]):
                running = False
        screen.fill(pygame.color.THECOLORS["black"])

        #draw screen, update physics objects
        #physics objects must be updated & drawn first so that sonar dots can detect red color
        space.debug_draw(draw_options)
        pygame.display.flip()
        dist = player.drawSonar()  #array of distance values, [left up right]
        screen.fill(pygame.color.THECOLORS["black"])

        #choose action
        action = nn.getAction()

        #do action
        player.rotate(0.01)

        #get reward
        reward = player.getReward()

        #update screen
        space.debug_draw(draw_options)
        pygame.display.flip()

        #get new dist
        dist = player.drawSonar()

        #train
        nn.getHighestQPrime()
        nn.train(123123)

        fps = 60
        dt = 1. / fps
        space.step(dt)
        clock.tick(fps)
        input()
예제 #11
0
파일: game.py 프로젝트: owad/thegame
class Game:
    
    def __init__(self):
        self.cfg = config.Config(file('config.ini'))
        self.game_start_time = time.time()
        self.game_speed = self.cfg.init_game_speed
        self.play_time = 0
        self.game_score = 0
        self.player_name = ''
        
        self.fps = self.cfg.fps
        pygame.init()
        try:
            if sys.argv[1] == 'fullscreen':
                info = pygame.display.Info()
                self.window_size = (info.current_w, info.current_h)
                pygame.display.set_mode(self.window_size, pygame.FULLSCREEN)
        except:
            self.window_size = (640, 480)
        self.screen = pygame.display.set_mode(self.window_size)
        self.clock = pygame.time.Clock()
        
        self.obstacles = Obstacles(self.screen, self.window_size)
        self.obstacles.gen_obstacles(1)
        self.player = Player(self.screen, self.window_size)
        self.bg = Background(self.screen)
    
    def display_text(self, text, x=None, y=None, font_size=40):
        myfont = pygame.font.SysFont("Arial Bold", font_size)
        label = myfont.render(text, 1, (255, 255, 255))
        if not x:
            x = self.window_size[0] / 2 - label.get_width() / 2
        if not y:
            y = self.window_size[1] / 3
        self.screen.blit(label, (x, y))
        return label

    def game_over(self):
        self.display_text('GAME OVER', None, 50)
        self.display_text("your score is %s" % self.game_score, None, 80)
        
    def game_score_display(self):
        self.play_time = time.time() - self.game_start_time
        self.display_text("Score: %s" % str(self.game_score), 10, 10)
    
    def game_score_save(self, key_index=None):
        if len(self.player_name) < 3:
            if key_index != None and key_index > 96 and key_index < 123:
                self.player_name += chr(key_index)
            self.display_text('Type your initials: %s' % self.player_name, None, 120, 30)
        elif len(self.player_name) == 3:
            writer = csv.writer(open('scores.txt', 'ab'))
            writer.writerow([self.player_name, self.game_score])
            self.player_name += '-1'
        elif int(self.player_name[-1:]) == 1:
            self.player_name = 'abc2'
            reader = csv.reader(open('scores.txt', 'rb'))
            scores = []
            for row in reader:
                scores.append((row[0], row[1]))
            scores.sort(None, operator.itemgetter(1), True)
            writer = csv.writer(open('scores.txt', 'wb'))
            writer.writerows(scores[:5])
        else:
            self.game_score_board()
            
    def game_score_board(self):
        reader = csv.reader(open('scores.txt', 'rb'))
        y = 120
        self.display_text("Top scores", None, y, 30)
        y = 150
        for row in reader:
            self.display_text('%s %s' % (row[0].upper(), row[1]), None, y, 30)
            y += 30
    
    def game_speedup(self, speedup=1):
        self.game_speed += speedup
    
    def draw(self, hide_scores=False):
        self.bg.draw()
        self.obstacles.draw()
        self.player.draw()
        if not hide_scores:
            self.game_score_display()
    
    def main(self):
        done = False
        stop = False
        gameover = False
        speedup_counter = self.fps
        while done == False:
            if not gameover:
                if self.player.hit_obstacle(self.obstacles.collection):
                    gameover = True
            key_index = None
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    done = True
                if event.type == pygame.KEYUP:
                    if event.key == pygame.K_ESCAPE:
                        done = True
                    if gameover:
                        key_index = event.key
                self.player.keyboard_move(event)
            self.player.mouse_move()
            
            self.obstacles.up(self.game_speed)
            if speedup_counter > 0:
                speedup_counter -= 1
            else:
                if int(self.play_time) % 5 == 0 and self.play_time != 0:
                    self.game_speedup()
                    if len(self.obstacles.collection) < self.cfg.max_obstacles:
                        self.obstacles.one_more()
                speedup_counter = self.fps
            if not stop:
                self.clock.tick(self.fps)
            
            self.draw(gameover)
            
            if gameover:
                self.game_over()
                self.game_score_save(key_index)
                stop = True
            else: 
                self.game_score += 1
            
            pygame.display.flip()
            self.clock.tick(self.fps)
예제 #12
0
    return False

def gen_rand(low, high, size):
    arrs = []
    for i in range(len(low)):
        span = high[i]-low[i]
        arrs.append((np.random.random(size)*span*1.1 + low[i]-span*0.05).reshape(-1,1))
    return np.concatenate(arrs, 1)

if __name__ == '__main__':
    obstacle_path, goal_path = "../hw3/world_obstacles.txt", "../hw3/goal.txt"
    # obstacle_path, goal_path = "world_obstacles.txt", "start_goal.txt"
    path = get_obstacle_course(obstacle_path)
    start, goal = get_start_and_goal(goal_path)

    obstacles = Obstacles(path.to_polygons())
    shapes = [Polygon(polyg) for polyg in path.to_polygons()]
    num_tests = 10000

    """ Unit test for line in obstacle """
    minp, maxp = path.vertices.min(0), path.vertices.max(0)
    step_size = 50

    starts = gen_rand(minp, maxp, num_tests)
    rands  = gen_rand(minp, maxp, num_tests)

    # starts, rands = starts.astype(int), rands.astype(int)

    dirs  = starts - rands
    ends  = starts + dirs / np.hypot(dirs[:,0], dirs[:,1])[:,None] * step_size
    paths = [Path([starts[i], ends[i]], [Path.MOVETO, Path.LINETO]) for i in range(num_tests)]
예제 #13
0
    return False

def gen_rand(low, high, size):
    arrs = []
    for i in range(len(low)):
        span = high[i]-low[i]
        arrs.append((np.random.random(size)*span*1.1 + low[i]-span*0.05).reshape(-1,1))
    return np.concatenate(arrs, 1)

if __name__ == '__main__':
    obstacle_path, goal_path = "../hw3/world_obstacles.txt", "../hw3/goal.txt"
    # obstacle_path, goal_path = "world_obstacles.txt", "start_goal.txt"
    path = get_obstacle_course(obstacle_path)
    start, goal = get_start_and_goal(goal_path)

    obstacles = Obstacles(path.to_polygons())
    shapes = [Polygon(polyg) for polyg in path.to_polygons()]
    num_tests = 10000

    """ Unit test for point in obstacle """
    minp, maxp = path.vertices.min(0), path.vertices.max(0)
    points = gen_rand(minp, maxp, num_tests)

    # points = points.astype(int)

    my_ans = [not obstacles.point_is_valid(*points[i]) for i in range(num_tests)]
    ex_ans = [shapely_contains(points[i]) for i in range(num_tests)]

    correct_positives, correct_negatives = [], []
    false_positives, false_negatives = [], []
    for i in range(num_tests):
예제 #14
0
"""
Main Program File to start 2D Drone Simulation
"""

import pygame
from environment import Environment
from obstacles import Obstacles
from drone import Drone

if __name__ == "__main__":

    # Initialize Environment
    env = Environment()

    # Initialize Obstacles
    obstacles = Obstacles(env)

    # Initialize Drone
    drone = Drone(env)

    # Main loop
    while env.running:

        # Update all environment variables first (dt)
        env.update()

        # for loop through the event queue
        for event in pygame.event.get():
            # Get Keys which are held down (easier for drone control)
            pressed = pygame.key.get_pressed()
            drone.check_user_input(pressed)
예제 #15
0
 def testMove(self):
     rider = Rider(rouleurShade, "green")
     self.logger.logMove(rider, (0, 0), (3, 1), Obstacles([]))
     self.display.displayRiders([rider])
     self.animate()
예제 #16
0
def run(obstacles, start, goal, step_size, max_size, plotter):
    circ_rad = min(step_size / 5, 5)
    final_pos = np.array(goal[:2])

    # Pre-compute for generating new random points in Q_free
    minp, maxp = obstacles.vertices.min(0), obstacles.vertices.max(0)
    span, offset = (maxp - minp) * 1.1, minp - (maxp - minp) * 0.05

    def gen_valid_rand(valid_function):
        """ Generates a valid q_rand in Q_free given a validity function """
        tmp = np.random.random(2) * span + offset
        while not valid_function(*tmp):
            tmp = np.random.random(2) * span + offset
        return tmp

    KD = KDTree(start)
    RRT = PathTree(start)
    circ1 = plotter.draw_circle(start, 1, time=1, zorder=5)
    obstacles = Obstacles(obstacles.to_polygons())

    trials = 0
    while KD.length < max_size:
        trials += 1
        circ1.remove()

        # Select a random point q_rand \in Q_free
        q_rand = gen_valid_rand(obstacles.point_is_valid) if np.random.randint(
            0, 100) > 5 else final_pos
        circ1 = plotter.draw_circle(q_rand, 5, time=0.01, zorder=5)

        # Find the nearest node and distance to it
        q_near, dist = KD.nearestNode(q_rand)

        # Generate the next node in the direction of q_rand
        if dist < step_size:
            if trials < 10: continue  # Prevents step_size too big bug
            q_next = tuple(q_rand)
        else:
            q_next = gen_next(q_near, q_rand, step_size)
            if not obstacles.point_is_valid(*q_next): continue

        # Check validity and update tree
        if obstacles.check_collisions((q_near, q_next)): continue

        KD.addNode(q_next)
        RRT.addPath(q_near, q_next)

        plotter.draw_line(q_near, q_next, color='k', zorder=1, update=False)
        plotter.draw_circle(q_next,
                            circ_rad,
                            edgecolor='k',
                            facecolor='w',
                            zorder=2)

        if not obstacles.check_collisions((q_next, goal)):
            # IF there is a direct line to the goal, then TAKE IT
            goal_distance = math.hypot(q_next[0] - goal[0],
                                       q_next[1] - goal[1])
            while goal_distance > 0:
                q_new = gen_next(q_next, goal, min(goal_distance, step_size))
                RRT.addPath(q_next, q_new)
                plotter.draw_line(q_next,
                                  q_new,
                                  color='k',
                                  zorder=1,
                                  update=False)
                plotter.draw_circle(q_new,
                                    circ_rad,
                                    edgecolor='k',
                                    facecolor='w',
                                    zorder=2)
                q_next = q_new
                goal_distance -= step_size
            break

        trials = 0

    print("n =", KD.length)

    cur = RRT[goal]
    while cur.parent:
        plotter.draw_line(cur, cur.parent, update=False, color='b', zorder=3)
        plotter.draw_circle(cur,
                            circ_rad * 1.5,
                            update=False,
                            facecolor='xkcd:green',
                            edgecolor='k',
                            zorder=4)
        cur = cur.parent
    plotter.update()
예제 #17
0
            plt.plot(points_x, points_y, color='red', linewidth=2)
            plt.plot(points_x[0], points_y[0], color='blue', zorder=0)

    plt.scatter(robot_pose.x, robot_pose.y, s=10**2, color='blue', zorder=1)
    plt.scatter(8, 8, s=10**2, color='green', zorder=1)
    if obstacle_pose is not (None):
        plt.scatter(obstacle_pose.x,
                    obstacle_pose.y,
                    s=10**2,
                    color='red',
                    zorder=1)
    plt.show()


# main function to intial RRT* algorithm
map_obstacles = Obstacles()
obstacles = []
obstacles.append(Polygon([(-1.1, 7.6), (1.1, 7.6), (1.1, -7.6), (-1.1, -7.6)]))
obstacles.append(Polygon([(-7.6, 1.1), (7.6, 1.1), (7.6, -1.1), (-7.6, -1.1)]))
obstacles.append(Polygon([(-10, 9.4), (-10, 10), (10, 10), (10, 9.4)]))
obstacles.append(Polygon([(-10, 10), (-10, -10), (-9.4, -10), (-9.4, 10)]))
obstacles.append(Polygon([(-10, -10), (10, -10), (10, -9.4), (-10, -9.4)]))
obstacles.append(Polygon([(9.4, -10), (10, -10), (10, 10), (9.4, 10)]))

map_obstacles.addNewObstacle(obstacles)
obstacles_list = map_obstacles.getObstacles()
cost_total = []

rrt = RRT(obstacles_list)
##################################################################################################################
예제 #18
0
def run(obstacles, start, goal, step_size, max_size, plotter):
    circ_rad = min(step_size/5, 5)
    final_pos = [np.array(goal), np.array(start)]

    # Pre-compute for generating new random points in Q_free
    minp, maxp   = obstacles.vertices.min(0), obstacles.vertices.max(0)
    span, offset = (maxp-minp)*1.1, minp-(maxp-minp)*0.05

    def gen_valid_rand(valid_function):
        """ Generates a valid q_rand in Q_free given a validity function """
        tmp     = np.random.random(2) * span + offset
        while not valid_function(*tmp):
            tmp = np.random.random(2) * span + offset
        return tmp

    KD    = [KDTree(start), KDTree(goal)]
    RRT   = [PathTree(start), PathTree(goal)]
    n     = 1
    rnd_display = False
    obstacles = Obstacles(obstacles.to_polygons())

    """
    • Expand tree T_1 randomly, add node q_new
    • Expand T_2 towards q_new
        • If tree T_2 connects to q_new, path formed else add a q_new for tree T_2
    • Now expand T_1 to q_new in tree T_2
    • Keep swapping T_1 and T_2 for expansion towards the
    other tree until they meet
    """

    trials = 0
    q_new, last_expanded = None, -1
    while KD[0].length + KD[1].length < 1000:
        trials += 1
        if rnd_display: circ1.remove(); rnd_display = False
        n = 1 - n
        
        # If the last expanded node was in the other tree, try expanding towards q_new
        if last_expanded != n and q_new is not None:
            q_near, dist = KD[n].nearestNode(q_new)
            q_next = gen_next(q_near, q_new, step_size) if dist>step_size else q_new
            
            # Expansion towards q_new is possible. Add to path and goal check
            if obstacles.point_is_valid(*q_next) and \
                not obstacles.check_collisions((q_near, q_next)):
                RRT[n].addPath(q_near, q_next)
                KD[n].addNode(q_next)
                
                plotter.draw_circle(q_next, circ_rad, edgecolor='k', facecolor='w', zorder=1)
                plotter.draw_line(q_near, q_next, color='kb'[n], zorder=1)

                if q_next == q_new: break # Path found
                q_new, last_expanded, trials = q_next, n, 0 # Update for next iteration
                continue

        # If last expanded node was not in the other tree or expansion to q_new not possible
        # Try to expand to q_rand if possible
        q_rand = gen_valid_rand(obstacles.point_is_valid) if np.random.randint(0,100)>5 else final_pos[n]
        rnd_display, circ1 = True, plotter.draw_circle(q_rand, 5, zorder=5)

        q_near, dist = KD[n].nearestNode(q_rand)
        if dist < step_size:
            if trials < 10: continue
            q_next = tuple(q_rand)
        else:
            q_next = gen_next(q_near, q_rand, step_size)
            if not obstacles.point_is_valid(*q_next): continue
        
        if obstacles.check_collisions((q_near, q_next)): continue

        KD[n].addNode(q_next)
        RRT[n].addPath(q_near, q_next)

        plotter.draw_line(q_near, q_next, color='kb'[n], zorder=1)
        plotter.draw_circle(q_next, circ_rad, edgecolor='k', facecolor='w', zorder=1)

        q_new, last_expanded, near_count = q_next, n, 0

    print("n =", KD[0].length + KD[1].length, "(%d, %d)"%(KD[0].length, KD[1].length))

    # Plot out goal path
    cur = RRT[0][tuple(q_next)]
    while cur.parent:
        plotter.draw_line(cur, cur.parent, update=False, color='y', zorder=3)
        plotter.draw_circle(cur, circ_rad*1.5, update=False, facecolor='xkcd:green', edgecolor='k', zorder=4)
        cur = cur.parent

    cur = RRT[1][tuple(q_next)]
    while cur.parent:
        plotter.draw_line(cur, cur.parent, update=False, color='y', zorder=3)
        plotter.draw_circle(cur, circ_rad*1.5, update=False, facecolor='xkcd:green', edgecolor='k', zorder=4)
        cur = cur.parent
    plotter.update()
예제 #19
0
 def testMoveOne(self):
     obstacles = Obstacles([])
     assert_equals([(0, 0), (1, 0)], findPath(obstacles, (0, 0), (1, 0)))
예제 #20
0
 def testMoveStraight(self):
     obstacles = Obstacles([])
     assert_equals([(0, 0), (1, 0), (2, 0), (3, 0), (4, 0)],
                   findPath(obstacles, (0, 0), (4, 0)))