mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-29 16:40:46 +08:00
zhm-real
312 lines
9.2 KiB
Python
312 lines
9.2 KiB
Python
"""
|
|
RRT_STAR_SMART 2D
|
|
@author: huiming zhou
|
|
"""
|
|
|
|
import os
|
|
import sys
|
|
import math
|
|
import random
|
|
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
import matplotlib.patches as patches
|
|
from scipy.spatial.transform import Rotation as Rot
|
|
|
|
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
|
"/../../Sampling_based_Planning/")
|
|
|
|
from Sampling_based_Planning.rrt_2D import env, plotting, utils
|
|
|
|
|
|
class Node:
|
|
def __init__(self, n):
|
|
self.x = n[0]
|
|
self.y = n[1]
|
|
self.parent = None
|
|
|
|
|
|
class RrtStarSmart:
|
|
def __init__(self, x_start, x_goal, step_len,
|
|
goal_sample_rate, search_radius, iter_max):
|
|
self.x_start = Node(x_start)
|
|
self.x_goal = Node(x_goal)
|
|
self.step_len = step_len
|
|
self.goal_sample_rate = goal_sample_rate
|
|
self.search_radius = search_radius
|
|
self.iter_max = iter_max
|
|
|
|
self.env = env.Env()
|
|
self.plotting = plotting.Plotting(x_start, x_goal)
|
|
self.utils = utils.Utils()
|
|
|
|
self.fig, self.ax = plt.subplots()
|
|
self.delta = self.utils.delta
|
|
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.V = [self.x_start]
|
|
self.beacons = []
|
|
self.beacons_radius = 2
|
|
self.direct_cost_old = np.inf
|
|
self.obs_vertex = self.utils.get_obs_vertex()
|
|
self.path = None
|
|
|
|
def planning(self):
|
|
n = 0
|
|
b = 2
|
|
InitPathFlag = False
|
|
self.ReformObsVertex()
|
|
|
|
for k in range(self.iter_max):
|
|
if k % 200 == 0:
|
|
print(k)
|
|
|
|
if (k - n) % b == 0 and len(self.beacons) > 0:
|
|
x_rand = self.Sample(self.beacons)
|
|
else:
|
|
x_rand = self.Sample()
|
|
|
|
x_nearest = self.Nearest(self.V, x_rand)
|
|
x_new = self.Steer(x_nearest, x_rand)
|
|
|
|
if x_new and not self.utils.is_collision(x_nearest, x_new):
|
|
X_near = self.Near(self.V, x_new)
|
|
self.V.append(x_new)
|
|
|
|
if X_near:
|
|
# choose parent
|
|
cost_list = [self.Cost(x_near) + self.Line(x_near, x_new) for x_near in X_near]
|
|
x_new.parent = X_near[int(np.argmin(cost_list))]
|
|
|
|
# rewire
|
|
c_min = self.Cost(x_new)
|
|
for x_near in X_near:
|
|
c_near = self.Cost(x_near)
|
|
c_new = c_min + self.Line(x_new, x_near)
|
|
if c_new < c_near:
|
|
x_near.parent = x_new
|
|
|
|
if not InitPathFlag and self.InitialPathFound(x_new):
|
|
InitPathFlag = True
|
|
n = k
|
|
|
|
if InitPathFlag:
|
|
self.PathOptimization(x_new)
|
|
if k % 5 == 0:
|
|
self.animation()
|
|
|
|
self.path = self.ExtractPath()
|
|
self.animation()
|
|
plt.plot([x for x, _ in self.path], [y for _, y in self.path], '-r')
|
|
plt.pause(0.01)
|
|
plt.show()
|
|
|
|
def PathOptimization(self, node):
|
|
direct_cost_new = 0.0
|
|
node_end = self.x_goal
|
|
|
|
while node.parent:
|
|
node_parent = node.parent
|
|
if not self.utils.is_collision(node_parent, node_end):
|
|
node_end.parent = node_parent
|
|
else:
|
|
direct_cost_new += self.Line(node, node_end)
|
|
node_end = node
|
|
|
|
node = node_parent
|
|
|
|
if direct_cost_new < self.direct_cost_old:
|
|
self.direct_cost_old = direct_cost_new
|
|
self.UpdateBeacons()
|
|
|
|
def UpdateBeacons(self):
|
|
node = self.x_goal
|
|
beacons = []
|
|
|
|
while node.parent:
|
|
near_vertex = [v for v in self.obs_vertex
|
|
if (node.x - v[0]) ** 2 + (node.y - v[1]) ** 2 < 9]
|
|
if len(near_vertex) > 0:
|
|
for v in near_vertex:
|
|
beacons.append(v)
|
|
|
|
node = node.parent
|
|
|
|
self.beacons = beacons
|
|
|
|
def ReformObsVertex(self):
|
|
obs_vertex = []
|
|
|
|
for obs in self.obs_vertex:
|
|
for vertex in obs:
|
|
obs_vertex.append(vertex)
|
|
|
|
self.obs_vertex = obs_vertex
|
|
|
|
def Steer(self, x_start, x_goal):
|
|
dist, theta = self.get_distance_and_angle(x_start, x_goal)
|
|
dist = min(self.step_len, dist)
|
|
node_new = Node((x_start.x + dist * math.cos(theta),
|
|
x_start.y + dist * math.sin(theta)))
|
|
node_new.parent = x_start
|
|
|
|
return node_new
|
|
|
|
def Near(self, nodelist, node):
|
|
n = len(self.V) + 1
|
|
r = 50 * math.sqrt((math.log(n) / n))
|
|
|
|
dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist]
|
|
X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2 and
|
|
not self.utils.is_collision(node, nodelist[ind])]
|
|
|
|
return X_near
|
|
|
|
def Sample(self, goal=None):
|
|
if goal is None:
|
|
delta = self.utils.delta
|
|
goal_sample_rate = self.goal_sample_rate
|
|
|
|
if np.random.random() > goal_sample_rate:
|
|
return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
|
|
np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
|
|
|
|
return self.x_goal
|
|
else:
|
|
R = self.beacons_radius
|
|
r = random.uniform(0, R)
|
|
theta = random.uniform(0, 2 * math.pi)
|
|
ind = random.randint(0, len(goal) - 1)
|
|
|
|
return Node((goal[ind][0] + r * math.cos(theta),
|
|
goal[ind][1] + r * math.sin(theta)))
|
|
|
|
def SampleFreeSpace(self):
|
|
delta = self.delta
|
|
|
|
if np.random.random() > self.goal_sample_rate:
|
|
return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
|
|
np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
|
|
|
|
return self.x_goal
|
|
|
|
def ExtractPath(self):
|
|
path = []
|
|
node = self.x_goal
|
|
|
|
while node.parent:
|
|
path.append([node.x, node.y])
|
|
node = node.parent
|
|
|
|
path.append([self.x_start.x, self.x_start.y])
|
|
|
|
return path
|
|
|
|
def InitialPathFound(self, node):
|
|
if self.Line(node, self.x_goal) < self.step_len:
|
|
return True
|
|
|
|
return False
|
|
|
|
@staticmethod
|
|
def Nearest(nodelist, n):
|
|
return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2
|
|
for nd in nodelist]))]
|
|
|
|
@staticmethod
|
|
def Line(x_start, x_goal):
|
|
return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y)
|
|
|
|
@staticmethod
|
|
def Cost(node):
|
|
cost = 0.0
|
|
if node.parent is None:
|
|
return cost
|
|
|
|
while node.parent:
|
|
cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
|
|
node = node.parent
|
|
|
|
return cost
|
|
|
|
@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 animation(self):
|
|
plt.cla()
|
|
self.plot_grid("rrt*-Smart, N = " + str(self.iter_max))
|
|
plt.gcf().canvas.mpl_connect(
|
|
'key_release_event',
|
|
lambda event: [exit(0) if event.key == 'escape' else None])
|
|
|
|
for node in self.V:
|
|
if node.parent:
|
|
plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")
|
|
|
|
if self.beacons:
|
|
theta = np.arange(0, 2 * math.pi, 0.1)
|
|
r = self.beacons_radius
|
|
|
|
for v in self.beacons:
|
|
x = v[0] + r * np.cos(theta)
|
|
y = v[1] + r * np.sin(theta)
|
|
plt.plot(x, y, linestyle='--', linewidth=2, color='darkorange')
|
|
|
|
plt.pause(0.01)
|
|
|
|
def plot_grid(self, name):
|
|
|
|
for (ox, oy, w, h) in self.obs_boundary:
|
|
self.ax.add_patch(
|
|
patches.Rectangle(
|
|
(ox, oy), w, h,
|
|
edgecolor='black',
|
|
facecolor='black',
|
|
fill=True
|
|
)
|
|
)
|
|
|
|
for (ox, oy, w, h) in self.obs_rectangle:
|
|
self.ax.add_patch(
|
|
patches.Rectangle(
|
|
(ox, oy), w, h,
|
|
edgecolor='black',
|
|
facecolor='gray',
|
|
fill=True
|
|
)
|
|
)
|
|
|
|
for (ox, oy, r) in self.obs_circle:
|
|
self.ax.add_patch(
|
|
patches.Circle(
|
|
(ox, oy), r,
|
|
edgecolor='black',
|
|
facecolor='gray',
|
|
fill=True
|
|
)
|
|
)
|
|
|
|
plt.plot(self.x_start.x, self.x_start.y, "bs", linewidth=3)
|
|
plt.plot(self.x_goal.x, self.x_goal.y, "rs", linewidth=3)
|
|
|
|
plt.title(name)
|
|
plt.axis("equal")
|
|
|
|
|
|
def main():
|
|
x_start = (18, 8) # Starting node
|
|
x_goal = (37, 18) # Goal node
|
|
|
|
rrt = RrtStarSmart(x_start, x_goal, 1.5, 0.10, 0, 1000)
|
|
rrt.planning()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|