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)
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
def __init__(self, track, riders, players): self.track = track self.riders = riders self.obstacles = Obstacles(riders) self.players = players self.arrivals = [] self.checkArrivals()
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
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)))
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()
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]
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()
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 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()
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)
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)]
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):
""" 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)
def testMove(self): rider = Rider(rouleurShade, "green") self.logger.logMove(rider, (0, 0), (3, 1), Obstacles([])) self.display.displayRiders([rider]) self.animate()
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()
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) ##################################################################################################################
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()
def testMoveOne(self): obstacles = Obstacles([]) assert_equals([(0, 0), (1, 0)], findPath(obstacles, (0, 0), (1, 0)))
def testMoveStraight(self): obstacles = Obstacles([]) assert_equals([(0, 0), (1, 0), (2, 0), (3, 0), (4, 0)], findPath(obstacles, (0, 0), (4, 0)))