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)
# This can help avoid the robot to be trapped at some undesirable position
if fail == max_fail_time:
path.pop()
Coef.pop()
x = path[-1]
print("--- %s seconds ---" % (time.time() - start_time))
# TODO : Post-processing
# Get rid of the small steps
# Generate trajectory from way-point
traj = []
X_plot = []
Z_plot = []
for i in range(len(path) - 1):
dx = (path[i + 1][0] - path[i][0]) / 100
for j in range(100):
x_x = path[i][0] + j * dx
x_z = Coef[i][0] * x_x**2 + Coef[i][1] * x_x + Coef[i][2]
traj.append((x_x, x_z))
X_plot.append(x_x)
Z_plot.append(x_z)
traj.append(path[-1])
plot = Plot("jumpleg")
plot.plot_path(X, traj)
plot.plot_obstacles(X, Obstacles)
plot.plot_start(X, x_init)
plot.plot_goal(X, x_goal)
plot.draw(auto_open=True)
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)