saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-29 16:40:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

update RRT

Browse Source
This commit is contained in:
zhm-real
2020-06-24 19:41:29 -07:00
parent 09fe3e787f
commit 6a637809f1
9 changed files with 181 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc
BIN
View File
Binary file not shown.
Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc
BIN
View File
Binary file not shown.
Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc
BIN
View File
Binary file not shown.
Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc
BIN
View File
Binary file not shown.
Sampling-based Planning/rrt_2D/env.py
+18 -19
View File
@@ -15,37 +15,36 @@ class Env:
@staticmethod
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)
[0, 0, 1, 30],
[0, 30, 50, 1],
[1, 0, 50, 1],
[50, 1, 1, 30]
]
return obs_boundary
@staticmethod
def obs_rectangle():
obs_rectangle = [
(13, 10, 5, 3),
(18, 4, 5, 4),
(22, 13, 6, 3),
(33, 15, 5, 3),
(42, 6, 5, 3)
[13, 10, 5, 3],
[18, 4, 5, 4],
[18, 20, 8, 5],
[22, 13, 6, 3],
[33, 15, 5, 3],
[42, 6, 5, 3]
# [42, 25, 1, 2],
# [45, 23, 2, 1],
# [46, 27, 1, 2]
]
return obs_rectangle
@staticmethod
def obs_circle():
obs_cir = [
(5, 10, 3),
(10, 22, 3.5),
(21, 23, 3),
(34, 9, 4),
(37, 23, 3),
(45, 20, 2)
[5, 10, 3],
[10, 22, 3.5],
[34, 9, 4],
[37, 23, 3],
[45, 20, 2]
]
return obs_cir
Sampling-based Planning/rrt_2D/plotting.py
+3 -1
View File
@@ -70,12 +70,14 @@ class Plotting:
@staticmethod
def plot_visited(nodelist, animation):
if animation:
count = 0
for node in nodelist:
count += 1
if node.parent:
plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.pause(0.001)
if count % 5 == 0: plt.pause(0.001)
else:
for node in nodelist:
if node.parent:
Sampling-based Planning/rrt_2D/rrt.py
+19 -30
View File
@@ -13,6 +13,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
from rrt_2D import env
from rrt_2D import plotting
from rrt_2D import utils
class Node:
@@ -23,16 +24,17 @@ class Node:
class Rrt:
def __init__(self, x_start, x_goal, expand_len, goal_sample_rate, iter_limit):
def __init__(self, x_start, x_goal, step_len, goal_sample_rate, iter_max):
self.xI = Node(x_start)
self.xG = Node(x_goal)
self.expand_len = expand_len
self.step_len = step_len
self.goal_sample_rate = goal_sample_rate
self.iter_limit = iter_limit
self.iter_max = iter_max
self.vertex = [self.xI]
self.env = env.Env()
self.plotting = plotting.Plotting(x_start, x_goal)
self.utils = utils.Utils()
self.x_range = self.env.x_range
self.y_range = self.env.y_range
@@ -41,25 +43,28 @@ class Rrt:
self.obs_boundary = self.env.obs_boundary
def planning(self):
for i in range(self.iter_limit):
for i in range(self.iter_max):
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):
if node_new and not self.utils.is_collision(node_near, node_new):
self.vertex.append(node_new)
dist, _ = self.get_distance_and_angle(node_new, self.xG)
if dist <= self.expand_len:
if dist <= self.step_len:
self.new_state(node_new, self.xG)
return self.extract_path(node_new)
return None
def random_state(self, goal_sample_rate):
delta = self.utils.delta
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 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.xG
def nearest_neighbor(self, node_list, n):
@@ -67,12 +72,11 @@ class Rrt:
for nd in node_list]))]
def new_state(self, node_start, node_end):
node_new = Node((node_start.x, node_start.y))
dist, theta = self.get_distance_and_angle(node_new, node_end)
dist, theta = self.get_distance_and_angle(node_start, node_end)
dist = min(self.expand_len, dist)
node_new.x += dist * math.cos(theta)
node_new.y += dist * math.sin(theta)
dist = min(self.step_len, dist)
node_new = Node((node_start.x + dist * math.cos(theta),
node_start.y + dist * math.sin(theta)))
node_new.parent = node_start
return node_new
@@ -87,21 +91,6 @@ class Rrt:
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
@@ -113,11 +102,11 @@ def main():
x_start = (2, 2) # Starting node
x_goal = (49, 28) # Goal node
rrt = Rrt(x_start, x_goal, 0.4, 0.05, 2000)
rrt = Rrt(x_start, x_goal, 0.6, 0.05, 3000)
path = rrt.planning()
if path:
rrt.plotting.animation(rrt.vertex, path)
rrt.plotting.animation(rrt.vertex, path, True)
else:
print("No Path Found!")
Sampling-based Planning/rrt_2D/rrt_star.py
+35 -31
View File
@@ -13,6 +13,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
from rrt_2D import env
from rrt_2D import plotting
from rrt_2D import utils
class Node:
@@ -24,18 +25,19 @@ class Node:
class RrtStar:
def __init__(self, x_start, x_goal, expand_len,
goal_sample_rate, search_radius, iter_limit):
def __init__(self, x_start, x_goal, step_len,
goal_sample_rate, search_radius, iter_max):
self.xI = Node(x_start)
self.xG = Node(x_goal)
self.expand_len = expand_len
self.step_len = step_len
self.goal_sample_rate = goal_sample_rate
self.search_radius = search_radius
self.iter_limit = iter_limit
self.iter_max = iter_max
self.vertex = [self.xI]
self.env = env.Env()
self.plotting = plotting.Plotting(x_start, x_goal)
self.utils = utils.Utils()
self.x_range = self.env.x_range
self.y_range = self.env.y_range
@@ -44,12 +46,12 @@ class RrtStar:
self.obs_boundary = self.env.obs_boundary
def planning(self):
for k in range(self.iter_limit):
for k in range(self.iter_max):
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):
if node_new and not self.utils.is_collision(node_near, node_new):
neighbor_index = self.find_near_neighbor(node_new)
if neighbor_index:
node_new = self.choose_parent(node_new, neighbor_index)
@@ -59,10 +61,28 @@ class RrtStar:
index = self.search_goal_parent()
return self.extract_path(self.vertex[index])
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
def random_state(self, goal_sample_rate):
delta = self.utils.delta
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 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.xG
def nearest_neighbor(self, node_list, n):
@@ -70,19 +90,18 @@ class RrtStar:
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)
dist, theta = self.get_distance_and_angle(node_start, node_goal)
node_new.x += dist * math.cos(theta)
node_new.y += dist * math.sin(theta)
dist = min(self.step_len, dist)
node_new = Node((node_start.x + dist * math.cos(theta),
node_start.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)
r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len)
dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex]
@@ -103,7 +122,7 @@ class RrtStar:
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]
node_index = [dist_list.index(i) for i in dist_list if i <= self.step_len]
if node_index:
cost_list = [dist_list[i] + self.vertex[i].cost for i in node_index]
@@ -140,21 +159,6 @@ class RrtStar:
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
@@ -166,7 +170,7 @@ 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)
rrt_star = RrtStar(x_start, x_goal, 1, 0.1, 10, 20000)
path = rrt_star.planning()
if path:
Sampling-based Planning/rrt_2D/utils.py
+106
View File
@@ -0,0 +1,106 @@
"""
utils for collision check
@author: huiming zhou
"""
import math
import numpy as np
import pyrr
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Sampling-based Planning/")
from rrt_2D import env
from rrt_2D.rrt import Node
class Utils:
def __init__(self):
self.env = env.Env()
self.delta = 0.2
self.obs_circle = self.env.obs_circle
self.obs_rectangle = self.env.obs_rectangle
self.obs_boundary = self.env.obs_boundary
self.obs_vertex = self.get_obs_vertex()
def get_obs_vertex(self):
delta = self.delta
obs_list = []
for (ox, oy, w, h) in self.obs_rectangle:
vertex_list = [[ox - delta, oy - delta],
[ox + w + delta, oy - delta],
[ox + w + delta, oy + h + delta],
[ox - delta, oy + h + delta]]
obs_list.append(vertex_list)
return obs_list
def is_intersect_segment(self, start, end, a, b):
o, d = self.get_ray(start, end)
v1 = [o[0] - a[0], o[1] - a[1]]
v2 = [b[0] - a[0], b[1] - a[1]]
v3 = [-d[1], d[0]]
div = np.dot(v2, v3)
if div == 0:
div = 0.01
t1 = np.linalg.norm(np.cross(v2, v1)) / div
t2 = np.dot(v1, v3) / div
if t1 >= 0 and 0 <= t2 <= 1:
shot = Node((o[0] + t1 * d[0], o[1] + t1 * d[1]))
dist_obs = self.get_dist(start, shot)
dist_seg = self.get_dist(start, end)
if dist_obs <= dist_seg:
return True
return False
def is_collision(self, start, end):
if self.is_inside_obs(start) or self.is_inside_obs(end):
return True
for (v1, v2, v3, v4) in self.obs_vertex:
if self.is_intersect_segment(start, end, v1, v2) \
or self.is_intersect_segment(start, end, v2, v3) \
or self.is_intersect_segment(start, end, v3, v4) \
or self.is_intersect_segment(start, end, v4, v1):
return True
return False
def is_inside_obs(self, node):
delta = self.delta
for (x, y, r) in self.obs_circle:
if math.hypot(node.x - x, node.y - y) <= r + delta:
return True
for (x, y, w, h) in self.obs_rectangle:
if 0 <= node.x - (x - delta) <= w + 2 * delta \
and 0 <= node.y - (y - delta) <= h + 2 * delta:
return True
for (x, y, w, h) in self.obs_boundary:
if 0 <= node.x - (x - delta) <= w + 2 * delta \
and 0 <= node.y - (y - delta) <= h + 2 * delta:
return True
return False
@staticmethod
def get_ray(start, end):
orig = [start.x, start.y]
direc = [end.x - start.x, end.y - start.y]
return orig, direc
@staticmethod
def get_dist(start, end):
return math.hypot(end.x - start.x, end.y - start.y)