def __init__(self):
self.X1, self.Y1, self.X2, self.Y2 = polygons_to_segments(
self.load_polygons("./paths.txt"))
self.isovist = iso.Isovist(
[self.listify_segs(self.X1, self.Y1, self.X2, self.Y2)])
self.intruder_start_goals = [((0.114, 0.362), (0.55, 0.848)),
((0.378, 0.128), (0.824, 0.562)),
((0.292, 0.034), (0.284, 0.842)),
((0.71, 0.202), (0.37, 0.864)),
((0.91, 0.832), (0.146, 0.572)),
((0.854, 0.598), (0.29, 0.076)),
((0.828, 0.408), (0.12, 0.41)),
((0.614, 0.882), (0.368, 0.152)),
((0.154, 0.608), (0.736, 0.208)),
((0.668, 0.934), (0.742, 0.14)),
((0.12, 0.504), (0.714, 0.018)),
((0.722, 0.808), (0.29, 0.14)),
((0.854, 0.424), (0.19, 0.68)),
((0.802, 0.152), (0.116, 0.456)),
((0.168, 0.158), (0.886, 0.736)),
((0.802, 0.692), (0.166, 0.456)),
((0.764, 0.246), (0.572, 0.866)),
((0.15, 0.69), (0.856, 0.46)),
((0.904, 0.864), (0.084, 0.502)),
((0.782, 0.274), (0.644, 0.88)),
((0.15, 0.184), (0.802, 0.3)),
((0.156, 0.204), (0.854, 0.516)),
((0.968, 0.032), (.9, .9))]
def create_chaser_model(seg_map=None, locs=None, isovist=None):
if locs is None:
locs = [[0.100, 1 - 0.900], [0.566, 1 - 0.854], [0.761, 1 - 0.665],
[0.523, 1 - 0.604], [0.241, 1 - 0.660], [0.425, 1 - 0.591],
[0.303, 1 - 0.429], [0.815, 1 - 0.402], [0.675, 1 - 0.075],
[0.432, 1 - 0.098]]
if seg_map is None:
seg_map = polygons_to_segments(load_polygons("./paths.txt"))
if isovist is None:
isovist = i.Isovist(load_isovist_map())
model = Chaser(isovist=isovist, locs=locs, seg_map=seg_map)
return model
def main():
polygonSegments = load_polygons()
# isovist stuff
isovist = iso.Isovist(polygonSegments)
X1, Y1, X2, Y2 = polygons_to_segments(load_polygons_here())
INTRUDER_SEEN_COUNT = 0
for K in range(100):
result = roll_out(isovist, X1, Y1, X2, Y2)
INTRUDER_SEEN_COUNT += result
print "%d/%d" % (INTRUDER_SEEN_COUNT, K + 1)
def __init__(self):
self.locs = [[0.100, 1 - 0.900], [0.566,
1 - 0.854], [0.761, 1 - 0.665],
[0.523, 1 - 0.604], [0.241,
1 - 0.660], [0.425, 1 - 0.591],
[0.303, 1 - 0.429], [0.815, 1 - 0.402],
[0.675, 1 - 0.075], [0.432, 1 - 0.098]]
self.locs = a2d(self.locs)
rx1, ry1, rx2, ry2 = polygons_to_segments(load_polygons("./paths.txt"))
self.my_rrt = lambda start_loc, goal_loc: run_rrt(
start_loc, goal_loc, rx1, ry1, rx2, ry2)
self.isovist = iso.Isovist([self.listify_segs(rx1, ry1, rx2, ry2)])
self.UAVLocation = (0.468, 1 - 0.764)
self.UAVForwardVector = (0.0, 0.010)
def __init__(self):
halls = [[0.4, 0.0], [0.6, 0.0], [0.6, 0.8], [1.0, 0.8], [1.0, 1.0],
[0.0, 1.0], [0.0, 0.8], [0.4, 0.8], [0.4, 0.0]]
self.locs = [[0.1, 0.9], [0.5, 0.1], [0.9, 0.9]]
halls = a2d(halls)
self.locs = a2d(self.locs)
# for compatibility with polygons_to_segments
tmp_halls = np.vstack((np.mean(halls, axis=0, keepdims=True), halls))
rx1, ry1, rx2, ry2 = polygons_to_segments([tmp_halls])
self.my_rrt = lambda start_loc, goal_loc: run_rrt(
start_loc, goal_loc, rx1, ry1, rx2, ry2)
self.isovist = iso.Isovist([self.listify_segs(rx1, ry1, rx2, ry2)])
#self.UAVLocation = ( 0.5, 0.11 )
#self.UAVLocation = ( 0.2, 0.82 )
self.UAVLocation = (0.2, 0.801)
self.UAVForwardVector = (0.0, 0.010)
def main():
ForceA = False
ForceB = False
ForceC = False
c = np.random.choice(3)
if c == 0: ForceA = True
if c == 1: ForceB = True
if c == 2: ForceC = True
# UAVLoc2 = True
if np.random.choice(2) == 0:
UAVLoc2 = True
else:
UAVLoc2 = False
if len(sys.argv) > 1 and sys.argv[1] == '1':
UAVLoc2 = False
if len(sys.argv) > 1 and sys.argv[1] == '2':
UAVLoc2 = True
if len(sys.argv) > 2 and sys.argv[2] == 'a':
ForceA = True
if len(sys.argv) > 2 and sys.argv[2] == 'c':
ForceC = True
if len(sys.argv) > 2 and sys.argv[2] == 'b':
ForceB = True
'''
xdim and ydim of the pygame screen
'''
xdim = 500
ydim = 500
backgroundFileName = "./cnts.png"
background = pygame.image.load(backgroundFileName)
background = pygame.transform.scale(background, (xdim, ydim))
backgroundRect = background.get_rect()
intruder_iso_radius = 70
array = np.zeros([xdim, ydim])
screen, clock = InitScreen(xdim, ydim)
polygonSegments = load_polygons()
# Clear canvas
screen.fill((255, 255, 255))
s = pygame.Surface((xdim, ydim)) # the size of your rect
s.set_alpha(0) # alpha level
s.fill((255, 255, 255)) # this fills the entire surface
screen.blit(s, (0, 0))
start_paint = (int(101), int(455))
end_paint = (int(336), int(47))
start = np.atleast_2d([(101 / 500.0), (455 / 500.0)]) #(101, 455)
end = np.atleast_2d([(336 / 500.0), (47 / 500.0)]) #(336, 47)
original_polygons = load_polygons_here()
X1, Y1, X2, Y2 = polygons_to_segments(original_polygons)
if ForceA:
block = "140 302 141 363 155 332"
X1, Y1, X2, Y2 = polygons_to_segments(
add_isovist_obstacle(block, original_polygons))
# if ForceB:
# block = "6 281 163 284 163 305 52 316 57 483 155 475 149 369 294 370 291 286 327 285 347 376 248 489 14 494 11 347"
# X1, Y1, X2, Y2 = polygons_to_segments(add_isovist_obstacle(block, original_polygons))
if ForceB:
blockC = "287 297 289 372 311 329"
blockA = "139 306 59 300 104 280"
temp = add_isovist_obstacle(blockA, original_polygons)
temp = add_isovist_obstacle(blockC, temp)
X1, Y1, X2, Y2 = polygons_to_segments(temp)
if ForceC:
block = "65 307 304 305 172 255"
X1, Y1, X2, Y2 = polygons_to_segments(
add_isovist_obstacle(block, original_polygons))
Update()
isovist = iso.Isovist(polygonSegments)
agentx = 262
agenty = 214
if UAVLoc2:
agentx = 406
agenty = 211
UAVLocation = (agentx, agenty)
mouseClick = None
UAVForwardVector = (2, 121) # looking south
intersections = [(262, 214), (236.0, 260), (286.0, 260)]
isovist_block = "262 214 236 260 286 260"
if UAVLoc2:
intersections = [(406, 211), (384, 254), (430, 254)]
isovist_block = "406 211 384 254 430 254"
# Get intended path
path = run_rrt(start, end, X1, Y1, X2, Y2)
intended_path = GetReadablePath(path, screen)
isIntruderFound, seen_loc = IsIntruderSeenByUAV(intended_path,
intersections)
isUAVFound, u_seen_loc = IsUAVSeenByIntruder(UAVLocation,
intruder_iso_radius,
intended_path)
DrawMiniIsovistForPath(intended_path,
screen,
end_paint,
iso_radius=intruder_iso_radius,
intersection=u_seen_loc,
UAVSawInt=isIntruderFound)
intruderColor = (255, 0, 0)
if not isIntruderFound:
intruderColor = (0, 255, 0)
# Draw Polygon for intersections (isovist)
isovist_surface = pygame.Surface((xdim, ydim))
isovist_surface.fill((255, 255, 255))
isovist_surface.set_alpha(100)
# JUST for drawing the isovist
pygame.draw.polygon(isovist_surface, intruderColor, intersections)
screen.blit(isovist_surface, isovist_surface.get_rect())
pygame.draw.circle(screen, (100, 100, 100), UAVLocation, 5)
pygame.draw.circle(screen, (0, 255, 0), start_paint, 7)
pygame.draw.circle(screen, (255, 0, 0), end_paint, 7)
# Draw segments
for polygon in polygonSegments:
for segment in polygon:
pygame.draw.line(screen, (0, 0, 0), segment[0], segment[1], 2)
# Get re-routed path
# if isIntruderFound:
# X1, Y1, X2, Y2 = polygons_to_segments(add_isovist_obstacle(isovist_block, original_polygons))
# #X1, Y1, X2, Y2 = polygons_to_segments(original_polygons)
# seen_rrt = np.atleast_2d( [(seen_loc[0]/500.0 ),(seen_loc[1]/500.0)] )
# path = run_rrt( seen_rrt, end, X1, Y1, X2, Y2)
# rerouted_path = GetReadablePath(path, screen)
#
# DrawMiniIsovistForPath(rerouted_path, screen, end_paint, iso_radius= intruder_iso_radius, reroute_color=(0, 255, 0))
if seen_loc != None:
pygame.draw.circle(screen, (255, 255, 0), seen_loc, 5)
if u_seen_loc != None:
pygame.draw.circle(screen, (100, 45, 134), u_seen_loc, 5)
#write legend
pygame.draw.circle(screen, (100, 100, 100), [10, 10], 5)
pygame.draw.circle(screen, (0, 255, 0), [10, 40], 7)
pygame.draw.circle(screen, (255, 0, 0), [10, 70], 7)
pygame.draw.circle(screen, (255, 255, 0), [10, 100], 5)
pygame.draw.circle(screen, (100, 45, 134), [10, 130], 5)
myfont = pygame.font.SysFont("comicsansms", 15)
# render text
label = myfont.render("UAV", 2, (0, 0, 0))
screen.blit(label, (30, 3))
label = myfont.render("start", 2, (0, 0, 0))
screen.blit(label, (30, 33))
label = myfont.render("goal", 2, (0, 0, 0))
screen.blit(label, (30, 63))
label = myfont.render("detection of intruder by UAV", 2, (0, 0, 0))
screen.blit(label, (30, 93))
label = myfont.render("detection of UAV by intruder", 2, (0, 0, 0))
screen.blit(label, (30, 123))
label = myfont.render("Intruder detecter UAV = " + str(isUAVFound), 2,
(0, 0, 0))
screen.blit(label, (232, 400))
label = myfont.render("UAV detected Intruder = " + str(isIntruderFound), 2,
(0, 0, 0))
screen.blit(label, (232, 440))
pfont = pygame.font.SysFont("comicsansms", 30)
label = pfont.render("A", 3, (150, 150, 150))
screen.blit(label, (90, 278))
label = pfont.render("B", 3, (150, 150, 150))
screen.blit(label, (250, 278))
label = pfont.render("C", 3, (150, 150, 150))
screen.blit(label, (395, 278))
Update()
return [isUAVFound, isIntruderFound, ForceA, ForceB, ForceC, UAVLoc2]
if intruder_seen:
print " Detection!"
print UAV_loc
print Int_loc
break
return results, intruder_seen
#
# ==========================================================================
# MAIN
# ==========================================================================
#
polygonSegments = load_polygons()
isovist = iso.Isovist( polygonSegments )
X1, Y1, X2, Y2 = polygons_to_segments(load_polygons_here())
model = Coolworld()
model.isovist = isovist
model.X1 = X1
model.Y1 = Y1
model.X2 = X2
model.Y2 = Y2
results = []
#for K in tqdm(xrange( 100 )):
for K in xrange( 100 ):
print "============================================="
print "ITERATION %d" % K
if __name__ == '__main__':
# XXX This is testing the runner model. We can view samples from the prior
# conditioned [on the variable list below]
#test_chaser()
# setup
locs = [[0.100, 1 - 0.900], [0.566, 1 - 0.854], [0.761, 1 - 0.665],
[0.523, 1 - 0.604], [0.241, 1 - 0.660], [0.425, 1 - 0.591],
[0.303, 1 - 0.429], [0.815, 1 - 0.402], [0.675, 1 - 0.075],
[0.432, 1 - 0.098]]
seg_map = polygons_to_segments(load_polygons("./paths.txt"))
isovist = i.Isovist(load_isovist_map())
polys, epolys = load_segs()
# # for writing the smart preplanned runner
# way_pts = [[0.100, 1-0.9], [.28,.20] , [.26, .32],[.16,.36], [.11, .057], [.179, .086], [.38, .87], [.432, 1-.098]]
# way_pts = np.asarray(way_pts)
# print way_pts
dets = []
simulation_Q_history = []
# TODO: for x simulations
runner_intercepted_cnt = 0
for x in xrange(5):
sim_id = str(int(time.time()))
detection, Q_history = run_simulation(sim_id, locs, seg_map, isovist,
polys, epolys)
def main():
'''
xdim and ydim of the pygame screen
'''
xdim = 500
ydim = 500
backgroundFileName = "./cnts.png"
background = pygame.image.load(backgroundFileName)
background = pygame.transform.scale(background, (xdim, ydim))
backgroundRect = background.get_rect()
array = np.zeros([xdim, ydim])
screen, clock = InitScreen(xdim, ydim)
polygonSegments = load_polygons()
# Clear canvas
screen.fill((255, 255, 255))
s = pygame.Surface((xdim, ydim)) # the size of your rect
s.set_alpha(0) # alpha level
s.fill((255, 255, 255)) # this fills the entire surface
screen.blit(s, (0, 0))
screen.blit(background, backgroundRect)
#### RRT STUFF
start_paint = (int(0.1 * 500), int(0.1 * 500))
end_paint = (int(0.9 * 500), int(0.9 * 500))
start = np.atleast_2d([(0.1), (0.1)])
end = np.atleast_2d([(0.9), (0.9)])
X1, Y1, X2, Y2 = polygons_to_segments(load_polygons_here())
# Draw segments
for polygon in polygonSegments:
for segment in polygon:
pygame.draw.line(screen, (225, 225, 225), segment[0], segment[1],
1)
#RRTPath = [(200, 120), ( 450, 200)]
#Draw hard coded RRT Path
#pygame.draw.line(screen, (0, 0, 255), RRTPath[0], RRTPath[1],2)
# for point in RRTPath:
# pygame.draw.circle(screen, (255,100,255), point, 5)
Update()
isovist = iso.Isovist(polygonSegments)
#(313, 115)
agentx = 313
agenty = 215
UAVLocation = (agentx, agenty)
mouseClick = None
# intended path
path = run_rrt(start, end, X1, Y1, X2, Y2)
while True:
if mouseClick != None:
UAVLocation = mouseClick
# Clear canvas
# screen.fill((255,255,255))
# s = pygame.Surface((xdim,ydim)) # the size of your rect
# s.set_alpha(0) # alpha level
# s.fill((255,255,255)) # this fills the entire surface
# screen.blit(s, (0,0))
screen.blit(background, backgroundRect)
# Draw segments ( map )
for polygon in polygonSegments:
for segment in polygon:
pygame.draw.line(screen, (225, 225, 225), segment[0],
segment[1], 1)
mouse = pygame.mouse.get_pos()
# for i in range(0, X1.shape[0]):
# pygame.draw.line(screen, (225, 0, 0), [ X1[i], Y1[i] ], [ X2[i], Y2[i] ],1 )
#print UAVLocation
path = run_rrt(start, end, X1, Y1, X2, Y2)
readable_path = []
for i in xrange(1, len(path)):
s_point = path[i - 1]
s_point = (int(s_point[0] * 500), int(s_point[1] * 500))
e_point = path[i]
e_point = (int(e_point[0] * 500), int(e_point[1] * 500))
pygame.draw.line(screen, (225, 225, 0), s_point, e_point, 1)
readable_path.append(s_point)
# getting directions
dirx = mouse[0] - UAVLocation[0]
diry = mouse[1] - UAVLocation[1]
direction = (dirx, diry)
pygame.draw.circle(screen, (0, 255, 0), start_paint, 10)
pygame.draw.circle(screen, (255, 0, 0), end_paint, 10)
UAVForwardVector = direction
isIntruderFound = isovist.IsIntruderSeen(readable_path,
UAVLocation,
UAVForwardVector,
UAVFieldOfVision=45)
intruderColor = (255, 0, 0)
if isIntruderFound:
intruderColor = (0, 255, 0)
# Draw Polygon for intersections (isovist)
isovist_surface = pygame.Surface((xdim, ydim))
isovist_surface.set_alpha(80)
# JUST for drawing the isovist
intersections = isovist.GetIsovistIntersections(
UAVLocation, UAVForwardVector)
if intersections != []:
pygame.draw.polygon(isovist_surface, intruderColor, intersections)
screen.blit(isovist_surface, isovist_surface.get_rect())
pygame.draw.circle(screen, (100, 100, 100), UAVLocation, 5)
#pygame.draw.circle(screen, (100,100,100), mouse, 5)
#isox,isoy = UpdateMovement(isox,isoy)
mouseClick = Update()
pygame.time.delay(10)
def main():
'''
xdim and ydim of the pygame screen
'''
xdim = 500
ydim = 500
backgroundFileName = "./cnts.png"
background = pygame.image.load(backgroundFileName)
background = pygame.transform.scale(background, (xdim, ydim))
backgroundRect = background.get_rect()
array = np.zeros([xdim, ydim])
screen, clock = InitScreen(xdim, ydim)
polygonSegments = load_polygons()
# isovist stuff
isovist = iso.Isovist( polygonSegments )
# Clear canvas
screen.fill((255,255,255))
s = pygame.Surface((xdim,ydim)) # the size of your rect
s.set_alpha(0) # alpha level
s.fill((255,255,255)) # this fills the entire surface
screen.blit(s, (0,0))
screen.blit(background, backgroundRect)
X1, Y1, X2, Y2 = polygons_to_segments(load_polygons_here())
for polygon in polygonSegments:
for segment in polygon:
pygame.draw.line(screen, (225, 225, 225), segment[0], segment[1],1)
Update()
INTRUDER_SEEN_COUNT = 0
UAV_start = paint_loc( (0.968,0.032))
UAV_end = paint_loc((0.016, 0.02))
#==== testing full isovist area
# test_start = paint_loc((0.53, 0.28))
# pygame.draw.circle(screen, (255,255,0), test_start, 7)
# isovist_surface = pygame.Surface((xdim,ydim))
# isovist_surface.set_alpha(50)
# intersections = isovist.GetIsovistIntersections(test_start, (0,10), full_iso=True)
# if intersections != []:
# pygame.draw.polygon(isovist_surface, (255,255,0), intersections)
# screen.blit(isovist_surface, isovist_surface.get_rect())
# print isovist_area(intersections)
# Update()
# print "Paused"
# raw_input()
#========
K = 0
while K <= 0:
# SAMPLE INTRUDER
IntruderStart = paint_loc((0.06, 0.236))
IntruderGoal = paint_loc((.9,.9))
pygame.draw.circle(screen, (0,255,0), IntruderStart, 5)
pygame.draw.circle(screen, (255,0,0), IntruderGoal, 5)
# GET INTRUDER PATH AND DRAW INTENDED PATH
# intruder_path = run_rrt( rrt_loc(IntruderStart), rrt_loc(IntruderGoal), X1, Y1, X2, Y2)
# DrawRRT(intruder_path, screen, IntruderGoal)
intruder_path = smart_intruder_rrt(IntruderStart, IntruderGoal, X1, Y1, X2, Y2, isovist, N=10, X=8)
#THINK
UAV_path = run_rrt( rrt_loc(UAV_start), rrt_loc(UAV_end), X1, Y1, X2, Y2)
DrawRRT(UAV_path, screen, UAV_end, highlight = (0,255,0))
#STEPS THE INTRUDER TAKES TO GOAL
e_point = None
intruder_walking_color = (255, 0, 0)
UAV_searching_color = (255, 0, 255)
UAV_current_step = 1
UAV_steps = 3
UAV_curr_loc = UAV_start
Intruder_curr_loc = IntruderStart
for i in xrange(1, len(intruder_path)+1):
#DRAW STEP FOR INTRUDER
if i == len(intruder_path): #and e_point != None:
pygame.draw.line(screen, intruder_walking_color, paint_loc(intruder_path[-1]), IntruderGoal, 1)
else:
s = intruder_path[i-1]
e = intruder_path[i]
pygame.draw.line(screen, intruder_walking_color, paint_loc(s), paint_loc(e), 1)
#INTRUDER CURRENT LOCATION
Intruder_curr_loc = paint_loc(e)
if Intruder_curr_loc == None:
Intruder_curr_loc = paint_loc(intruder_path[-1])
#FOR EVERY 1 STEP OF INTRUDER, N STEPS FOR UAV
stepsToGo = UAV_current_step+UAV_steps
if len(UAV_path) - UAV_current_step < UAV_steps:
stepsToGo = len(UAV_path) + 1
for j in xrange(UAV_current_step, stepsToGo):
if j == len(UAV_path): #and e_point != None:
pygame.draw.line(screen, UAV_searching_color, paint_loc(UAV_path[-1]), UAV_end, 1)
else:
s = UAV_path[j-1]
e = UAV_path[j]
pygame.draw.line(screen, UAV_searching_color, paint_loc(s), paint_loc(e), 1)
# UAV CURRENT LOCATION
UAV_curr_loc = paint_loc(e)
if UAV_curr_loc == None:
UAV_curr_loc = paint_loc(UAV_path[-1])
if dist(Intruder_curr_loc, UAV_curr_loc) <= 70:
fv = direction(paint_loc(e),paint_loc(s))
if not(fv[0] == 0 or fv[1] == 0):
intersections = isovist.GetIsovistIntersections(UAV_curr_loc, fv)
intruder_seen = isovist.FindIntruderAtPoint(Intruder_curr_loc, intersections)
if intruder_seen:
INTRUDER_SEEN_COUNT += 1
print "INTRUDER SEEN!"
#raw_input()
Update()
pygame.time.delay(10)
UAV_current_step += UAV_steps
if stepsToGo >= len(UAV_path):
south = 50
#THINK AGAIN!
UAV_start = go_south(UAV_start, amt=south)
UAV_end = go_south(UAV_end, amt=south)
if UAV_start[1] >= 500:
UAV_start = paint_loc( (0.968,0.032))
UAV_end = paint_loc((0.016, 0.02))
temp = UAV_start
UAV_start = UAV_end
UAV_end = temp
UAV_path = run_rrt( rrt_loc(UAV_start), rrt_loc(UAV_end), X1, Y1, X2, Y2)
DrawRRT(UAV_path, screen, UAV_end, highlight = (0,255,0))
UAV_current_step = 1
Update()
pygame.time.delay(10)
K += 1
# index = getPathIndex()
# print index
myfont = pygame.font.SysFont("arial", 20)
while True:
mouseClick = Update()
if mouseClick != None:
loc = mouseClick[0]/500.0, mouseClick[1]/500.0
print loc,","
pygame.draw.circle(screen, (255,255,0), mouseClick, 7)
label = myfont.render(str(loc), 3, (255,255,0))
label_loc = (mouseClick[0] -30, mouseClick[1] - 20)
screen.blit(label,label_loc)
pygame.time.delay(10)
for i in range(rx1.shape[0]):
result.append([(rx1[i], ry1[i]), (rx2[i], ry2[i])])
return result
def load_data():
try:
with open("rrt_paths-new.dat") as f:
x = pickle.load(f)
except:
x = []
return x
X1, Y1, X2, Y2 = polygons_to_segments(load_polygons())
isovist = iso.Isovist([listify_segs(X1, Y1, X2, Y2)])
#===================GIVEN DATA==========================
#candidate UAV locations (10)
cand_uav_locs = [(0.532, 0.28), (0.66, 0.472), (0.242, 0.484), (0.294, 0.666),
(0.458, 0.752), (0.584, 0.64), (0.55, 0.1), (0.364, 0.194),
(0.47, 0.466), (0.742, 0.682)]
w = [0.08, 0.18, 0.10, 0.10, 0.04, 0.13, 0.08, 0.14, 0.10, 0.05]
#set start and goal locations
start = np.atleast_2d([(0.1), (0.1)])
end = np.atleast_2d([(0.9), (0.9)])
rrt_paths = load_data()
