def sovle_rrt(src, dst):
# x_init = (10.7016,0.472867,0.813236 ) # starting location
# x_goal = (-5,0,1) # goal location
x_init = tuple([x * 10 for x in x_init])
x_goal = tuple([x * 10 for x in x_goal])
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)
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)