mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-29 08:34:46 +08:00
update RRT_2D
This commit is contained in:
zhm-real
Binary file not shown.
Binary file not shown.
@@ -5,17 +5,14 @@ from rrt_2D import env
|
||||
|
||||
|
||||
class Plotting:
|
||||
def __init__(self, xI, xG):
|
||||
self.xI, self.xG = xI, xG
|
||||
def __init__(self, x_start, x_goal):
|
||||
self.xI, self.xG = x_start, x_goal
|
||||
self.env = env.Env()
|
||||
self.obs_bound = self.env.obs_boundary
|
||||
self.obs_circle = self.env.obs_circle
|
||||
self.obs_rectangle = self.env.obs_rectangle
|
||||
|
||||
def animation(self, nodelist, path, animation=False):
|
||||
if path is None:
|
||||
print("No path found!")
|
||||
return
|
||||
self.plot_grid("RRT")
|
||||
self.plot_visited(nodelist, animation)
|
||||
self.plot_path(path)
|
||||
@@ -55,6 +52,7 @@ class Plotting:
|
||||
|
||||
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")
|
||||
|
||||
@@ -72,9 +70,8 @@ class Plotting:
|
||||
if node.parent:
|
||||
plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
|
||||
|
||||
|
||||
@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()
|
||||
plt.show()
|
||||
|
||||
@@ -1,200 +0,0 @@
|
||||
from rrt_2D import env
|
||||
from rrt_2D import plotting
|
||||
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
|
||||
class Node:
|
||||
def __init__(self, n):
|
||||
self.x = n[0]
|
||||
self.y = n[1]
|
||||
self.cost = 0.0
|
||||
self.parent = None
|
||||
|
||||
|
||||
class RRT:
|
||||
def __init__(self, xI, xG):
|
||||
self.xI = Node(xI)
|
||||
self.xG = Node(xG)
|
||||
self.expand_len = 1
|
||||
self.goal_sample_rate = 0.05
|
||||
self.connect_dist = 10
|
||||
self.iterations = 5000
|
||||
self.node_list = [self.xI]
|
||||
|
||||
self.env = env.Env()
|
||||
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_circle
|
||||
self.obs_rectangle = self.env.obs_rectangle
|
||||
self.obs_boundary = self.env.obs_boundary
|
||||
|
||||
self.path = self.planning()
|
||||
self.plotting.animation(self.node_list, self.path, False)
|
||||
|
||||
def planning(self):
|
||||
for k in range(self.iterations):
|
||||
node_rand = self.random_state()
|
||||
node_near = self.nearest_neighbor(self.node_list, node_rand)
|
||||
node_new = self.new_state(node_near, node_rand)
|
||||
|
||||
if not self.check_collision(node_new):
|
||||
neighbor_index = self.find_near_neighbor(node_new)
|
||||
node_new = self.choose_parent(node_new, neighbor_index)
|
||||
if node_new:
|
||||
self.node_list.append(node_new)
|
||||
self.rewire(node_new, neighbor_index)
|
||||
|
||||
# if self.dis_to_goal(self.node_list[-1]) <= self.expand_len:
|
||||
# self.new_state(self.node_list[-1], self.xG)
|
||||
# return self.extract_path()
|
||||
|
||||
index = self.search_best_goal_node()
|
||||
self.xG.parent = self.node_list[index]
|
||||
return self.extract_path()
|
||||
|
||||
def random_state(self):
|
||||
if np.random.random() > self.goal_sample_rate:
|
||||
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 nearest_neighbor(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]))]
|
||||
|
||||
def new_state(self, node_start, node_goal):
|
||||
node_new = Node((node_start.x, node_start.y))
|
||||
dist, theta = self.get_distance_and_angle(node_new, node_goal)
|
||||
dist = min(self.expand_len, dist)
|
||||
|
||||
node_new.x += dist * math.cos(theta)
|
||||
node_new.y += dist * math.sin(theta)
|
||||
node_new.parent = node_start
|
||||
|
||||
return node_new
|
||||
|
||||
def find_near_neighbor(self, node_new):
|
||||
n = len(self.node_list) + 1
|
||||
r = min(self.connect_dist * math.sqrt((math.log(n) / n)), self.expand_len)
|
||||
|
||||
dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.node_list]
|
||||
node_index = [dist_table.index(d) for d in dist_table if d <= r]
|
||||
|
||||
return node_index
|
||||
|
||||
def choose_parent(self, node_new, neighbor_index):
|
||||
if not neighbor_index:
|
||||
return None
|
||||
|
||||
cost = []
|
||||
|
||||
for i in neighbor_index:
|
||||
node_near = self.node_list[i]
|
||||
node_mid = self.new_state(node_near, node_new)
|
||||
|
||||
if node_mid and not self.check_collision(node_mid):
|
||||
cost.append(self.update_cost(node_near, node_mid))
|
||||
else:
|
||||
cost.append(float("inf"))
|
||||
|
||||
if min(cost) != float('inf'):
|
||||
index = int(np.argmin(cost))
|
||||
neighbor_min = neighbor_index[index]
|
||||
node_new = self.new_state(self.node_list[neighbor_min], node_new)
|
||||
node_new.cost = min(cost)
|
||||
return node_new
|
||||
|
||||
return None
|
||||
|
||||
def search_best_goal_node(self):
|
||||
dist_to_goal_list = [self.dis_to_goal(n) for n in self.node_list]
|
||||
goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.expand_len]
|
||||
|
||||
return goal_inds[0]
|
||||
# safe_goal_inds = []
|
||||
# for goal_ind in goal_inds:
|
||||
# t_node = self.new_state(self.node_list[goal_ind], self.xG)
|
||||
# if self.check_collision(t_node):
|
||||
# safe_goal_inds.append(goal_ind)
|
||||
#
|
||||
# if not safe_goal_inds:
|
||||
# print('hahhah')
|
||||
# return None
|
||||
#
|
||||
# min_cost = min([self.node_list[i].cost for i in safe_goal_inds])
|
||||
# for i in safe_goal_inds:
|
||||
# if self.node_list[i].cost == min_cost:
|
||||
# self.xG.parent = self.node_list[i]
|
||||
|
||||
def rewire(self, node_new, neighbor_index):
|
||||
for i in neighbor_index:
|
||||
node_near = self.node_list[i]
|
||||
node_edge = self.new_state(node_new, node_near)
|
||||
if not node_edge:
|
||||
continue
|
||||
|
||||
node_edge.cost = self.update_cost(node_new, node_near)
|
||||
collision = self.check_collision(node_edge)
|
||||
improved_cost = node_near.cost > node_edge.cost
|
||||
|
||||
if not collision and improved_cost:
|
||||
self.node_list[i] = node_edge
|
||||
self.propagate_cost_to_leaves(node_new)
|
||||
|
||||
def update_cost(self, node_start, node_end):
|
||||
dist, theta = self.get_distance_and_angle(node_start, node_end)
|
||||
return node_start.cost + dist
|
||||
|
||||
def propagate_cost_to_leaves(self, parent_node):
|
||||
for node in self.node_list:
|
||||
if node.parent == parent_node:
|
||||
node.cost = self.update_cost(parent_node, node)
|
||||
self.propagate_cost_to_leaves(node)
|
||||
|
||||
def extract_path(self):
|
||||
path = [[self.xG.x, self.xG.y]]
|
||||
node = self.xG
|
||||
while node.parent is not None:
|
||||
path.append([node.x, node.y])
|
||||
node = node.parent
|
||||
path.append([node.x, node.y])
|
||||
|
||||
return path
|
||||
|
||||
def dis_to_goal(self, node_cal):
|
||||
return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y)
|
||||
|
||||
def check_collision(self, node_end):
|
||||
if node_end is None:
|
||||
return True
|
||||
|
||||
for (ox, oy, r) in self.obs_circle:
|
||||
if math.hypot(node_end.x - ox, node_end.y - oy) <= r:
|
||||
return True
|
||||
|
||||
for (ox, oy, w, h) in self.obs_rectangle:
|
||||
if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
|
||||
return True
|
||||
|
||||
for (ox, oy, w, h) in self.obs_boundary:
|
||||
if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
@staticmethod
|
||||
def get_distance_and_angle(node_start, node_end):
|
||||
dx = node_end.x - node_start.x
|
||||
dy = node_end.y - node_start.y
|
||||
return math.hypot(dx, dy), math.atan2(dy, dx)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
x_Start = (2, 2) # Starting node
|
||||
x_Goal = (49, 28) # Goal node
|
||||
|
||||
rrt = RRT(x_Start, x_Goal)
|
||||
@@ -12,17 +12,17 @@ class Node:
|
||||
self.parent = None
|
||||
|
||||
|
||||
class RRT:
|
||||
def __init__(self, xI, xG):
|
||||
self.xI = Node(xI)
|
||||
self.xG = Node(xG)
|
||||
self.expand_len = 0.4
|
||||
self.goal_sample_rate = 0.05
|
||||
self.iterations = 5000
|
||||
self.node_list = [self.xI]
|
||||
class Rrt:
|
||||
def __init__(self, x_start, x_goal, expand_len, goal_sample_rate, iter_limit):
|
||||
self.xI = Node(x_start)
|
||||
self.xG = Node(x_goal)
|
||||
self.expand_len = expand_len
|
||||
self.goal_sample_rate = goal_sample_rate
|
||||
self.iter_limit = iter_limit
|
||||
self.vertex = [self.xI]
|
||||
|
||||
self.env = env.Env()
|
||||
self.plotting = plotting.Plotting(xI, xG)
|
||||
self.plotting = plotting.Plotting(x_start, x_goal)
|
||||
|
||||
self.x_range = self.env.x_range
|
||||
self.y_range = self.env.y_range
|
||||
@@ -30,33 +30,31 @@ class RRT:
|
||||
self.obs_rectangle = self.env.obs_rectangle
|
||||
self.obs_boundary = self.env.obs_boundary
|
||||
|
||||
self.path = self.planning()
|
||||
self.plotting.animation(self.node_list, self.path)
|
||||
|
||||
def planning(self):
|
||||
for i in range(self.iterations):
|
||||
node_rand = self.random_state()
|
||||
node_near = self.nearest_neighbor(self.node_list, node_rand)
|
||||
for i in range(self.iter_limit):
|
||||
node_rand = self.random_state(self.goal_sample_rate)
|
||||
node_near = self.nearest_neighbor(self.vertex, node_rand)
|
||||
node_new = self.new_state(node_near, node_rand)
|
||||
|
||||
if not self.check_collision(node_new):
|
||||
self.node_list.append(node_new)
|
||||
if node_new and not self.check_collision(node_new):
|
||||
self.vertex.append(node_new)
|
||||
dist, _ = self.get_distance_and_angle(node_new, self.xG)
|
||||
|
||||
if self.dis_to_goal(self.node_list[-1]) <= self.expand_len:
|
||||
self.new_state(self.node_list[-1], self.xG)
|
||||
return self.extract_path(self.node_list)
|
||||
if dist <= self.expand_len:
|
||||
self.new_state(node_new, self.xG)
|
||||
return self.extract_path(node_new)
|
||||
|
||||
return None
|
||||
|
||||
def random_state(self):
|
||||
if np.random.random() > self.goal_sample_rate:
|
||||
def random_state(self, goal_sample_rate):
|
||||
if np.random.random() > goal_sample_rate:
|
||||
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 nearest_neighbor(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]))]
|
||||
return self.vertex[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
|
||||
for nd in node_list]))]
|
||||
|
||||
def new_state(self, node_start, node_end):
|
||||
node_new = Node((node_start.x, node_start.y))
|
||||
@@ -69,9 +67,9 @@ class RRT:
|
||||
|
||||
return node_new
|
||||
|
||||
def extract_path(self, nodelist):
|
||||
def extract_path(self, node_end):
|
||||
path = [(self.xG.x, self.xG.y)]
|
||||
node_now = nodelist[-1]
|
||||
node_now = node_end
|
||||
|
||||
while node_now.parent is not None:
|
||||
node_now = node_now.parent
|
||||
@@ -79,13 +77,7 @@ class RRT:
|
||||
|
||||
return path
|
||||
|
||||
def dis_to_goal(self, node_cal):
|
||||
return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y)
|
||||
|
||||
def check_collision(self, node_end):
|
||||
if node_end is None:
|
||||
return True
|
||||
|
||||
for (ox, oy, r) in self.obs_circle:
|
||||
if math.hypot(node_end.x - ox, node_end.y - oy) <= r:
|
||||
return True
|
||||
@@ -107,8 +99,18 @@ class RRT:
|
||||
return math.hypot(dx, dy), math.atan2(dy, dx)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
x_Start = (2, 2) # Starting node
|
||||
x_Goal = (49, 28) # Goal node
|
||||
def main():
|
||||
x_start = (2, 2) # Starting node
|
||||
x_goal = (49, 28) # Goal node
|
||||
|
||||
rrt = RRT(x_Start, x_Goal)
|
||||
rrt = Rrt(x_start, x_goal, 0.4, 0.05, 2000)
|
||||
path = rrt.planning()
|
||||
|
||||
if path:
|
||||
rrt.plotting.animation(rrt.vertex, path)
|
||||
else:
|
||||
print("No Path Found!")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
@@ -0,0 +1,169 @@
|
||||
from rrt_2D import env
|
||||
from rrt_2D import plotting
|
||||
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
|
||||
class Node:
|
||||
def __init__(self, n):
|
||||
self.x = n[0]
|
||||
self.y = n[1]
|
||||
self.cost = 0.0
|
||||
self.parent = None
|
||||
|
||||
|
||||
class RrtStar:
|
||||
def __init__(self, x_start, x_goal, expand_len,
|
||||
goal_sample_rate, search_radius, iter_limit):
|
||||
self.xI = Node(x_start)
|
||||
self.xG = Node(x_goal)
|
||||
self.expand_len = expand_len
|
||||
self.goal_sample_rate = goal_sample_rate
|
||||
self.search_radius = search_radius
|
||||
self.iter_limit = iter_limit
|
||||
self.vertex = [self.xI]
|
||||
|
||||
self.env = env.Env()
|
||||
self.plotting = plotting.Plotting(x_start, x_goal)
|
||||
|
||||
self.x_range = self.env.x_range
|
||||
self.y_range = self.env.y_range
|
||||
self.obs_circle = self.env.obs_circle
|
||||
self.obs_rectangle = self.env.obs_rectangle
|
||||
self.obs_boundary = self.env.obs_boundary
|
||||
|
||||
def planning(self):
|
||||
for k in range(self.iter_limit):
|
||||
node_rand = self.random_state(self.goal_sample_rate)
|
||||
node_near = self.nearest_neighbor(self.vertex, node_rand)
|
||||
node_new = self.new_state(node_near, node_rand)
|
||||
|
||||
if node_new and not self.check_collision(node_new):
|
||||
neighbor_index = self.find_near_neighbor(node_new)
|
||||
if neighbor_index:
|
||||
node_new = self.choose_parent(node_new, neighbor_index)
|
||||
self.vertex.append(node_new)
|
||||
self.rewire(node_new, neighbor_index)
|
||||
|
||||
index = self.search_goal_parent()
|
||||
return self.extract_path(self.vertex[index])
|
||||
|
||||
def random_state(self, goal_sample_rate):
|
||||
if np.random.random() > goal_sample_rate:
|
||||
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 nearest_neighbor(self, node_list, n):
|
||||
return self.vertex[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
|
||||
for nd in node_list]))]
|
||||
|
||||
def new_state(self, node_start, node_goal):
|
||||
node_new = Node((node_start.x, node_start.y))
|
||||
dist, theta = self.get_distance_and_angle(node_new, node_goal)
|
||||
dist = min(self.expand_len, dist)
|
||||
|
||||
node_new.x += dist * math.cos(theta)
|
||||
node_new.y += dist * math.sin(theta)
|
||||
node_new.parent = node_start
|
||||
|
||||
return node_new
|
||||
|
||||
def find_near_neighbor(self, node_new):
|
||||
n = len(self.vertex) + 1
|
||||
r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.expand_len)
|
||||
|
||||
dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex]
|
||||
|
||||
return [dist_table.index(d) for d in dist_table if d <= r]
|
||||
|
||||
def choose_parent(self, node_new, neighbor_index):
|
||||
cost = []
|
||||
|
||||
for i in neighbor_index:
|
||||
node_neighbor = self.vertex[i]
|
||||
cost.append(self.get_new_cost(node_neighbor, node_new))
|
||||
|
||||
cost_min_index = neighbor_index[int(np.argmin(cost))]
|
||||
node_new = self.new_state(self.vertex[cost_min_index], node_new)
|
||||
node_new.cost = min(cost)
|
||||
|
||||
return node_new
|
||||
|
||||
def search_goal_parent(self):
|
||||
dist_list = [math.hypot(n.x - self.xG.x, n.y - self.xG.y) for n in self.vertex]
|
||||
node_index = [dist_list.index(i) for i in dist_list if i <= self.expand_len]
|
||||
|
||||
if node_index:
|
||||
cost_list = [dist_list[i] + self.vertex[i].cost for i in node_index]
|
||||
return node_index[int(np.argmin(cost_list))]
|
||||
|
||||
return None
|
||||
|
||||
def rewire(self, node_new, neighbor_index):
|
||||
for i in neighbor_index:
|
||||
node_neighbor = self.vertex[i]
|
||||
new_cost = self.get_new_cost(node_new, node_neighbor)
|
||||
|
||||
if node_neighbor.cost > new_cost:
|
||||
self.vertex[i] = self.new_state(node_new, node_neighbor)
|
||||
self.propagate_cost_to_leaves(node_new)
|
||||
|
||||
def get_new_cost(self, node_start, node_end):
|
||||
dist, _ = self.get_distance_and_angle(node_start, node_end)
|
||||
return node_start.cost + dist
|
||||
|
||||
def propagate_cost_to_leaves(self, parent_node):
|
||||
for node in self.vertex:
|
||||
if node.parent == parent_node:
|
||||
node.cost = self.get_new_cost(parent_node, node)
|
||||
self.propagate_cost_to_leaves(node)
|
||||
|
||||
def extract_path(self, node_end):
|
||||
path = [[self.xG.x, self.xG.y]]
|
||||
node = node_end
|
||||
while node.parent is not None:
|
||||
path.append([node.x, node.y])
|
||||
node = node.parent
|
||||
path.append([node.x, node.y])
|
||||
|
||||
return path
|
||||
|
||||
def check_collision(self, node_end):
|
||||
for (ox, oy, r) in self.obs_circle:
|
||||
if math.hypot(node_end.x - ox, node_end.y - oy) <= r:
|
||||
return True
|
||||
|
||||
for (ox, oy, w, h) in self.obs_rectangle:
|
||||
if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
|
||||
return True
|
||||
|
||||
for (ox, oy, w, h) in self.obs_boundary:
|
||||
if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
@staticmethod
|
||||
def get_distance_and_angle(node_start, node_end):
|
||||
dx = node_end.x - node_start.x
|
||||
dy = node_end.y - node_start.y
|
||||
return math.hypot(dx, dy), math.atan2(dy, dx)
|
||||
|
||||
|
||||
def main():
|
||||
x_start = (2, 2) # Starting node
|
||||
x_goal = (49, 28) # Goal node
|
||||
|
||||
rrt_star = RrtStar(x_start, x_goal, 1, 0.1, 10, 5000)
|
||||
path = rrt_star.planning()
|
||||
|
||||
if path:
|
||||
rrt_star.plotting.animation(rrt_star.vertex, path)
|
||||
else:
|
||||
print("No Path Found!")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user