mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-30 00:50:46 +08:00
update RRT
This commit is contained in:
zhm-real
+45
-126
@@ -1,18 +1,14 @@
|
||||
import env
|
||||
import plotting
|
||||
import node
|
||||
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import matplotlib.patches as patches
|
||||
|
||||
|
||||
class Node:
|
||||
def __init__(self, x, y):
|
||||
self.x = x
|
||||
self.y = y
|
||||
def __init__(self, n):
|
||||
self.x = n[0]
|
||||
self.y = n[1]
|
||||
self.path_x = []
|
||||
self.path_y = []
|
||||
self.parent = None
|
||||
@@ -20,96 +16,39 @@ class Node:
|
||||
|
||||
class RRT:
|
||||
def __init__(self, xI, xG):
|
||||
self.xI = Node(xI[0], xI[1])
|
||||
self.xG = node.Node(xG[0], xG[1])
|
||||
self.xI = Node(xI)
|
||||
self.xG = Node(xG)
|
||||
self.expand_len = 0.8
|
||||
self.goal_sample_rate = 0.05
|
||||
self.iterations = 5000
|
||||
self.node_list = [self.xI]
|
||||
|
||||
self.env = env.Env()
|
||||
# self.plotting = plotting.Plotting(xI, xG)
|
||||
self.plotting = plotting.Plotting(xI, xG)
|
||||
|
||||
self.x_range = self.env.x_range
|
||||
self.y_range = self.env.y_range
|
||||
self.obs_circle = self.env.obs
|
||||
self.obs_rectangle = self.env.obs_boundary
|
||||
|
||||
# self.obs_boundary = self.env.obs_boundary
|
||||
# self.obs_circle = self.env.obs_circle
|
||||
|
||||
self.obstacleList = [
|
||||
(5, 5, 1),
|
||||
(3, 6, 2),
|
||||
(3, 8, 2),
|
||||
(3, 10, 2),
|
||||
(7, 5, 2),
|
||||
(9, 5, 2),
|
||||
(8, 10, 1)
|
||||
] # [x, y, radius]
|
||||
|
||||
self.expand_range = 0.8
|
||||
self.goal_sample_rate = 0.05
|
||||
self.iterations = 500
|
||||
self.node_list = []
|
||||
|
||||
path = self.planning()
|
||||
|
||||
if path is None:
|
||||
print("No path!")
|
||||
else:
|
||||
print("get it!")
|
||||
|
||||
self.draw_graph()
|
||||
|
||||
plt.plot([x[0] for x in path], [x[1] for x in path], '-r')
|
||||
plt.grid(True)
|
||||
plt.pause(0.01) # Need for Mac
|
||||
plt.show()
|
||||
self.path = self.planning()
|
||||
self.plotting.animation(self.node_list, self.path)
|
||||
|
||||
def planning(self):
|
||||
self.node_list = [self.xI]
|
||||
for i in range(self.iterations):
|
||||
node_rand = self.generate_random_node()
|
||||
node_near = self.get_nearest_node(self.node_list, node_rand)
|
||||
node_new = self.new_node(node_near, node_rand, self.expand_len)
|
||||
|
||||
node_new = self.new_node(node_near, node_rand, self.expand_range)
|
||||
|
||||
if not self.check_collision(node_new, self.obstacleList):
|
||||
if not self.check_collision(node_new, self.obs_circle, self.obs_rectangle):
|
||||
self.node_list.append(node_new)
|
||||
|
||||
self.draw_graph(node_rand)
|
||||
|
||||
if self.cal_dis_to_goal(self.node_list[-1]) <= self.expand_range:
|
||||
node_end = self.new_node(self.node_list[-1], node.Node(self.xG.x, self.xG.y), self.expand_range)
|
||||
if not self.check_collision(node_end, self.obstacleList):
|
||||
return self.extract_path(self.node_list)
|
||||
if self.cal_dis_to_goal(self.node_list[-1]) <= self.expand_len:
|
||||
self.new_node(self.node_list[-1], self.xG, self.expand_len)
|
||||
return self.extract_path(self.node_list)
|
||||
|
||||
return None
|
||||
|
||||
def draw_graph(self, rnd=None):
|
||||
plt.clf()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if rnd is not None:
|
||||
plt.plot(rnd.x, rnd.y, "^k")
|
||||
for node_x in self.node_list:
|
||||
if node_x.parent:
|
||||
plt.plot(node_x.path_x, node_x.path_y, "-g")
|
||||
|
||||
for (ox, oy, size) in self.obstacleList:
|
||||
self.plot_circle(ox, oy, size)
|
||||
|
||||
plt.plot(self.xI.x, self.xI.y, "xr")
|
||||
plt.plot(self.xG.x, self.xG.y, "xr")
|
||||
plt.axis("equal")
|
||||
plt.axis([-2, 15, -2, 15])
|
||||
plt.grid(True)
|
||||
plt.pause(0.01)
|
||||
|
||||
@staticmethod
|
||||
def plot_circle(x, y, size, color="-b"): # pragma: no cover
|
||||
deg = list(range(0, 360, 5))
|
||||
deg.append(0)
|
||||
xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
|
||||
yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
|
||||
plt.plot(xl, yl, color)
|
||||
|
||||
def extract_path(self, nodelist):
|
||||
path = [(self.xG.x, self.xG.y)]
|
||||
node_now = nodelist[-1]
|
||||
@@ -121,20 +60,20 @@ class RRT:
|
||||
return path
|
||||
|
||||
def cal_dis_to_goal(self, node_cal):
|
||||
return math.hypot((node_cal.x - self.xG.x), (node_cal.y - self.xG.y))
|
||||
return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y)
|
||||
|
||||
def new_node(self, node_start, node_goal, expand_range):
|
||||
new_node = node.Node(node_start.x, node_start.y)
|
||||
def new_node(self, node_start, node_goal, expand_len):
|
||||
new_node = Node((node_start.x, node_start.y))
|
||||
d, theta = self.calc_distance_and_angle(new_node, node_goal)
|
||||
|
||||
new_node.path_x = [new_node.x]
|
||||
new_node.path_y = [new_node.y]
|
||||
|
||||
if d < expand_range:
|
||||
expand_range = d
|
||||
if d < expand_len:
|
||||
expand_len = d
|
||||
|
||||
new_node.x += expand_range * math.cos(theta)
|
||||
new_node.y += expand_range * math.sin(theta)
|
||||
new_node.x += expand_len * math.cos(theta)
|
||||
new_node.y += expand_len * math.sin(theta)
|
||||
new_node.path_x.append(new_node.x)
|
||||
new_node.path_y.append(new_node.y)
|
||||
|
||||
@@ -144,58 +83,38 @@ class RRT:
|
||||
|
||||
def generate_random_node(self):
|
||||
if np.random.random() > self.goal_sample_rate:
|
||||
return node.Node(np.random.uniform(self.x_range[0], self.x_range[1]),
|
||||
np.random.uniform(self.y_range[0], self.y_range[1]))
|
||||
else:
|
||||
return node.Node(self.xG.x, self.xG.y)
|
||||
return Node((np.random.uniform(self.x_range[0], self.x_range[1]),
|
||||
np.random.uniform(self.y_range[0], self.y_range[1])))
|
||||
return self.xG
|
||||
|
||||
def get_nearest_node(self, node_list, node_random):
|
||||
dlist = [(nod.x - node_random.x) ** 2 + (nod.y - node_random.y) ** 2
|
||||
for nod in node_list]
|
||||
minind = dlist.index(min(dlist))
|
||||
|
||||
return self.node_list[minind]
|
||||
def get_nearest_node(self, node_list, n):
|
||||
return self.node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
|
||||
for nd in node_list]))]
|
||||
|
||||
@staticmethod
|
||||
def calc_distance_and_angle(from_node, to_node):
|
||||
dx = to_node.x - from_node.x
|
||||
dy = to_node.y - from_node.y
|
||||
d = math.hypot(dx, dy)
|
||||
theta = math.atan2(dy, dx)
|
||||
return d, theta
|
||||
return math.hypot(dx, dy), math.atan2(dy, dx)
|
||||
|
||||
def check_collision(self, node_check, obstacleList):
|
||||
|
||||
if node_check is None:
|
||||
@staticmethod
|
||||
def check_collision(node_end, obs_circle, obs_rectangle):
|
||||
if node_end is None:
|
||||
return True
|
||||
|
||||
for (ox, oy, size) in obstacleList:
|
||||
dx_list = [ox - x for x in node_check.path_x]
|
||||
dy_list = [oy - y for y in node_check.path_y]
|
||||
d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]
|
||||
for (ox, oy, r) in obs_circle:
|
||||
if math.hypot(node_end.x - ox, node_end.y - oy) <= r:
|
||||
return True
|
||||
|
||||
if min(d_list) <= size ** 2:
|
||||
return True # collision
|
||||
for (ox, oy, w, h) in obs_rectangle:
|
||||
if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
|
||||
return True
|
||||
|
||||
return False # safe
|
||||
#
|
||||
# def check_collision(self, node_check):
|
||||
# for obs in self.obs_boundary:
|
||||
# dx = node_check.x - obs[0]
|
||||
# dy = node_check.y - obs[1]
|
||||
# if 0 <= dx <= obs[2] and 0 <= dy <= obs[2]:
|
||||
# return True
|
||||
#
|
||||
# for obs in self.obs_circle:
|
||||
# d = (node_check.x - obs[0]) ** 2 + (node_check.y - obs[1]) ** 2
|
||||
# if d <= obs[2] ** 2:
|
||||
# return True
|
||||
#
|
||||
# return False
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
x_Start = (0, 0) # Starting node
|
||||
x_Goal = (6, 10) # Goal node
|
||||
x_Start = (15, 5) # Starting node
|
||||
x_Goal = (45, 25) # Goal node
|
||||
|
||||
rrt = RRT(x_Start, x_Goal)
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,48 +1,34 @@
|
||||
import numpy as np
|
||||
|
||||
|
||||
class Env:
|
||||
def __init__(self):
|
||||
self.x_range = (-2, 15) # size of background
|
||||
self.y_range = (-2, 15)
|
||||
# self.obs_boundary = self.obs_boundary(self.x_range, self.y_range)
|
||||
# self.obs_circle = self.obs_circle()
|
||||
self.x_range = (0, 50)
|
||||
self.y_range = (0, 30)
|
||||
self.obs_boundary = self.obs_boundary()
|
||||
self.obs = self.obs_circle()
|
||||
|
||||
@staticmethod
|
||||
def obs_boundary(x, y):
|
||||
w = 1
|
||||
obs_boundary = []
|
||||
|
||||
for i in np.linspace(x[0], x[1], (x[1] - x[0]) // w + 1):
|
||||
obs_boundary.append((i, y[0], w))
|
||||
for i in np.linspace(x[0], x[1], (x[1] - x[0]) // w + 1):
|
||||
obs_boundary.append((i, y[1], w))
|
||||
for j in np.linspace(y[0] + 1, y[1] - w, (y[1] - y[0] - 2 * w) // w + 1):
|
||||
obs_boundary.append((x[0], j, w))
|
||||
for j in np.linspace(y[0] + 1, y[1] - w, (y[1] - y[0] - 2 * w) // w + 1):
|
||||
obs_boundary.append((x[1], j, w))
|
||||
|
||||
for i in np.linspace(10, 20, 10 // w + 1):
|
||||
obs_boundary.append((i, 15, w))
|
||||
for j in np.linspace(1, 14, 13 // w + 1):
|
||||
obs_boundary.append((20, j, w))
|
||||
|
||||
for j in np.linspace(15, 29, 14 // w + 1):
|
||||
obs_boundary.append((30, j, w))
|
||||
for j in np.linspace(1, 14, 13 // w + 1):
|
||||
obs_boundary.append((40, j, w))
|
||||
def obs_boundary():
|
||||
obs_boundary = [
|
||||
(0, 0, 1, 30),
|
||||
(0, 30, 50, 1),
|
||||
(1, 0, 50, 1),
|
||||
(50, 1, 1, 30),
|
||||
(20, 1, 1, 15),
|
||||
(10, 15, 10, 1),
|
||||
(30, 15, 1, 15),
|
||||
(40, 1, 1, 15)
|
||||
]
|
||||
|
||||
return obs_boundary
|
||||
|
||||
@staticmethod
|
||||
def obs_circle():
|
||||
obs_cir = [
|
||||
(8, 8, 3),
|
||||
(10, 23, 3),
|
||||
(20, 25, 2.5),
|
||||
(30, 7, 3),
|
||||
(40, 25, 2),
|
||||
(43, 20, 2.5)
|
||||
(5, 10, 3),
|
||||
(10, 22, 3.5),
|
||||
(21, 23, 3),
|
||||
(34, 9, 4),
|
||||
(37, 23, 3),
|
||||
(45, 20, 2)
|
||||
]
|
||||
|
||||
return obs_cir
|
||||
|
||||
@@ -1,7 +0,0 @@
|
||||
class Node:
|
||||
def __init__(self, x, y):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.path_x = []
|
||||
self.path_y = []
|
||||
self.parent = None
|
||||
@@ -8,35 +8,23 @@ class Plotting:
|
||||
self.xI, self.xG = xI, xG
|
||||
self.env = env.Env()
|
||||
self.obs_bound = self.env.obs_boundary
|
||||
self.obs_circle = self.env.obs_circle
|
||||
self.obs_circle = self.env.obs
|
||||
|
||||
def animation(self, nodelist, node_rand=None):
|
||||
plt.clf()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if node_rand is not None:
|
||||
plt.plot(node_rand.x, node_rand.y, "^k")
|
||||
def animation(self, nodelist, path):
|
||||
if path is None:
|
||||
print("No path found!")
|
||||
return
|
||||
|
||||
for node in nodelist:
|
||||
if node.parent:
|
||||
plt.plot(node.path_x, node.path_y, "-g")
|
||||
|
||||
plt.plot(self.xI[0], self.xI[1], "b*")
|
||||
plt.plot(self.xG[0], self.xG[1], "g*")
|
||||
|
||||
plt.axis("equal")
|
||||
plt.axis([-5, 55, -5, 35])
|
||||
plt.title("RRT")
|
||||
plt.grid(True)
|
||||
plt.pause(0.01)
|
||||
self.plot_visited(nodelist)
|
||||
self.plot_path(path)
|
||||
|
||||
def plot_grid(self, name):
|
||||
fig, ax = plt.subplots()
|
||||
|
||||
for x in self.obs_bound:
|
||||
ax.add_patch(
|
||||
patches.Rectangle(
|
||||
(x[0], x[1]), x[2], x[2],
|
||||
(x[0], x[1]), x[2], x[3],
|
||||
edgecolor='black',
|
||||
facecolor='black',
|
||||
fill=True
|
||||
@@ -47,44 +35,28 @@ class Plotting:
|
||||
ax.add_patch(
|
||||
patches.Circle(
|
||||
(x[0], x[1]), x[2],
|
||||
edgecolor='gray',
|
||||
edgecolor='black',
|
||||
facecolor='gray',
|
||||
fill=True
|
||||
)
|
||||
)
|
||||
|
||||
plt.plot(self.xI[0], self.xI[1], "b*")
|
||||
plt.plot(self.xG[0], self.xG[1], "g*")
|
||||
|
||||
plt.plot(self.xI[0], self.xI[1], "bs", linewidth=3)
|
||||
plt.plot(self.xG[0], self.xG[1], "gs", linewidth=3)
|
||||
plt.title(name)
|
||||
plt.axis("equal")
|
||||
plt.show()
|
||||
|
||||
def plot_visited(self, visited):
|
||||
visited.remove(self.xI)
|
||||
count = 0
|
||||
@staticmethod
|
||||
def plot_visited(nodelist):
|
||||
for node in nodelist:
|
||||
if node.parent:
|
||||
plt.plot(node.path_x, node.path_y, "-g")
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.pause(0.001)
|
||||
|
||||
for x in visited:
|
||||
count += 1
|
||||
plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o')
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
|
||||
if count < len(visited) / 3:
|
||||
length = 15
|
||||
elif count < len(visited) * 2 / 3:
|
||||
length = 30
|
||||
else:
|
||||
length = 45
|
||||
|
||||
if count % length == 0: plt.pause(0.001)
|
||||
|
||||
def plot_path(self, path):
|
||||
path.remove(self.xI)
|
||||
path.remove(self.xG)
|
||||
path_x = [path[i][0] for i in range(len(path))]
|
||||
path_y = [path[i][1] for i in range(len(path))]
|
||||
|
||||
plt.plot(path_x, path_y, linewidth='3', color='r', marker='o')
|
||||
@staticmethod
|
||||
def plot_path(path):
|
||||
plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2)
|
||||
plt.pause(0.01)
|
||||
plt.show()
|
||||
|
||||
Reference in New Issue
Block a user