def bi_rrt_runtest(start,goal,filename,r,prc):
'''
returns pth from bi directional rrt
r -> discretization of the connection to check if the
'''
collision=True
boundary, blocks = load_map('./maps/'+filename+'.txt')
X_dimensions=np.array([(boundary[0,0],boundary[0,3]),\
(boundary[0,1],boundary[0,4]),(boundary[0,2],boundary[0,5])])
x_start=(start[0],start[1],start[2])
x_goal=(goal[0],goal[1],goal[2])
obstacles=get_obstacle(blocks)
X = SearchSpace(X_dimensions,obstacles)
Q=[(0.2,6),(0.2*2**0.5,18),(0.2*3**0.5,2)]
#Q = np.array([(8, 4)])
max_samples=2000000
rewire_count = 26 # optional, number of nearby branches to rewire
# probability of checking for a connection to goal
t0=tic()
rrt = RRTStarBidirectionalHeuristic(X, Q, x_start,x_goal, max_samples, r, prc, rewire_count)
path = rrt.rrt_star_bid_h()
toc(t0,'planning')
arr_path=np.array(path)
collision=check_collision(arr_path,blocks)
success=False
if(collision==False):
print('yes')
np.save('bi_rrt_'+filename+'.npy',arr_path)
length=pathlength_rrt(filename)
fig, ax, hb, hs, hg = draw_map(boundary, blocks, start, goal)
ax.plot(arr_path[:,0],arr_path[:,1],arr_path[:,2],'r-')
success=True
return success,length
def rrt_runtest(start,goal,filename,r,prc):
collision=True
boundary, blocks = load_map('./maps/'+filename+'.txt')
X_dimensions=np.array([(boundary[0,0],boundary[0,3]),\
(boundary[0,1],boundary[0,4]),(boundary[0,2],boundary[0,5])])
x_start=(start[0],start[1],start[2])
x_goal=(goal[0],goal[1],goal[2])
obstacles=get_obstacle(blocks) #get obstacles
X = SearchSpace(X_dimensions,obstacles) #define state space
Q=[(0.1,6),(0.1*2**0.5,18),(0.1*3**0.5,2)] # edge lengths
max_samples=2000000 # max samples
rewire_count = 26 # optional, number of nearby branches to rewire
# probability of checking for a connection to goal
t0=tic()
rrt = RRTStar(X, Q, x_start,x_goal, max_samples, r, prc, rewire_count) #get path
path= rrt.rrt_star()
toc(t0,'planning')
arr_path=np.array(path)
collision=check_collision(arr_path,blocks)
length=0
success=False
if(collision==False):
print('yes')
np.save('rrt_'+filename+'.npy',arr_path)
length=pathlength_rrt(filename)
fig, ax, hb, hs, hg = draw_map(boundary, blocks, start, goal)
ax.plot(arr_path[:,0],arr_path[:,1],arr_path[:,2],'r-')
success=True
return success,length
def sovle_rrt(src, dst):
# x_init = (10.7016,0.472867,0.813236 ) # starting location
# x_goal = (-5,0,1) # goal location
x_init = src
x_goal = dst
x_init = tuple([x * 10 for x in x_init])
x_goal = tuple([x * 10 for x in x_goal])
X_dimensions[2][0] = x_init[2]
X_dimensions[2][1] = x_init[2] + 1
print('src, dst= ', x_init, x_goal)
Q = np.array([(2, 1)]) # length of tree edges
r = 1 # length of smallest edge to check for intersection with obstacles
max_samples = 1024 # max number of samples to take before timing out
rewire_count = 32 # optional, number of nearby branches to rewire
prc = 0.01 # probability of checking for a connection to goal
# create Search Space
X = SearchSpace(X_dimensions, Obstacles)
# create rrt_search
# for t in range(100):
# a=time.time()
rrt = RRTStarBidirectionalHeuristic(X, Q, x_init, x_goal, max_samples, r,
prc, rewire_count)
path = rrt.rrt_star_bid_h()
for j in range(len(path)):
path[j] = [path[j][i] / 10 for i in range(len(path[j]))]
# b=time.time()
# print('time=', b-a)
print(path)
# # plot
# plot = Plot("rrt_star_bid_h_3d")
# plot.plot_tree(X, rrt.trees)
# if path is not None:
# plot.plot_path(X, path)
# plot.plot_obstacles(X, Obstacles)
# plot.plot_start(X, x_init)
# plot.plot_goal(X, x_goal)
# plot.draw(auto_open=True)
return path
from src.rrt.rrt import RRT
from src.search_space.search_space import SearchSpace
from src.utilities.obstacle_generation import generate_random_obstacles
from src.utilities.plotting import Plot
X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space
x_init = (0, 0) # starting location
x_goal = (100, 100) # goal location
Q = np.array([(8, 4)]) # length of tree edges
r = 1 # length of smallest edge to check for intersection with obstacles
max_samples = 1024 # max number of samples to take before timing out
prc = 0.1 # probability of checking for a connection to goal
# create search space
X = SearchSpace(X_dimensions)
n = 50
Obstacles = generate_random_obstacles(X, x_init, x_goal, n)
# create rrt_search
rrt = RRT(X, Q, x_init, x_goal, max_samples, r, prc)
path = rrt.rrt_search()
# plot
plot = Plot("rrt_2d_with_random_obstacles")
plot.plot_tree(X, rrt.trees)
if path is not None:
plot.plot_path(X, path)
plot.plot_obstacles(X, Obstacles)
plot.plot_start(X, x_init)
plot.plot_goal(X, x_goal)
plot.draw(auto_open=True)
# obstacles
Obstacles = np.array([(20, 20, 20, 40, 40, 40), (20, 20, 60, 40, 40, 80),
(20, 60, 20, 40, 80, 40), (60, 60, 20, 80, 80, 40),
(60, 20, 20, 80, 40, 40), (60, 20, 60, 80, 40, 80),
(20, 60, 60, 40, 80, 80), (60, 60, 60, 80, 80, 80)])
x_init = (0, 0, 0) # starting location
x_goal = (100, 100, 100) # goal location
Q = np.array([(8, 4)]) # length of tree edges
r = 1 # length of smallest edge to check for intersection with obstacles
max_samples = 1024 # max number of samples to take before timing out
rewire_count = 32 # optional, number of nearby branches to rewire
prc = 0.1 # probability of checking for a connection to goal
# create Search Space
X = SearchSpace(X_dimensions, Obstacles)
# create rrt_search
rrt = RRTStar(X, Q, x_init, x_goal, max_samples, r, prc, rewire_count)
path = rrt.rrt_star()
# plot
plot = Plot("rrt_star_3d")
plot.plot_tree(X, rrt.trees)
if path is not None:
plot.plot_path(X, path)
plot.plot_obstacles(X, Obstacles)
plot.plot_start(X, x_init)
plot.plot_goal(X, x_goal)
plot.draw(auto_open=True)
# creating sample space
x_sample = [0]
z_sample = []
for i in range(len(Obstacles)):
x_sample.append(Obstacles[i][0])
x_sample.append(Obstacles[i][2])
z_sample.append(0)
z_sample.append(Obstacles[i][3])
x_sample.append(200)
z_sample.append(0)
# Huristic used for choosing jump direction
obstacle_last = Obstacles[-1][0]
# create search space
X = SearchSpace(X_dimensions, Obstacles)
path = [x_init]
x = x_init
Coef = []
# Used for reachability guided sample
lower_range = 10
upper_range = 30
# Used for collision checking
segments = 500
max_fail_time = 50
# Used for goal detection
goal_margin = 5
import numpy as np
from src.rrt.rrt import RRT
from src.search_space.search_space import SearchSpace
from src.utilities.plotting import Plot
# x right
# y down
# z forward
dimensions = np.array([(-5, 5), (-5, 5), (-1, 9)])
obstacles = np.array([(-5, -0.5, 4, 5, 0.5, 5)])
start = (0, 0, 0)
goal = (0, 0, 9)
q = [(0.5, 3)]
r = 0.1
max_samples = 128
prc = 0.1
space = SearchSpace(dimensions, obstacles)
rrt = RRT(space, q, start, goal, max_samples, r, prc)
path = rrt.rrt_search()
plot = Plot("eoc_rrt")
plot.plot_tree(space, rrt.trees)
if path is not None:
plot.plot_path(space, path)
plot.plot_obstacles(space, obstacles)
plot.plot_start(space, start)
plot.plot_goal(space, goal)
plot.draw(auto_open=True)
