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