github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 02:28:03 -05:00
Code Issues Packages Projects Releases Wiki Activity

Bug RRT* fix, issues #382 and #383 (#401)

Browse Source
This commit is contained in:
Rafael Rojas
2020-10-01 13:15:52 +02:00
committed by GitHub
parent 60e9e8f39b
commit 2b316502e3
2 changed files with 131 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
PathPlanning/RRT/rrt.py
View File

@@ -32,8 +32,15 @@ class RRT:
self.path_y = []
self.parent = None
def __init__(self, start, goal, obstacle_list, rand_area,
expand_dis=3.0, path_resolution=0.5, goal_sample_rate=5, max_iter=500):
def __init__(self,
start,
goal,
obstacle_list,
rand_area,
expand_dis=3.0,
path_resolution=0.5,
goal_sample_rate=5,
max_iter=500):
"""
Setting Parameter
@@ -75,8 +82,10 @@ class RRT:
if animation and i % 5 == 0:
self.draw_graph(rnd_node)
if self.calc_dist_to_goal(self.node_list[-1].x, self.node_list[-1].y) <= self.expand_dis:
final_node = self.steer(self.node_list[-1], self.end, self.expand_dis)
if self.calc_dist_to_goal(self.node_list[-1].x,
self.node_list[-1].y) <= self.expand_dis:
final_node = self.steer(self.node_list[-1], self.end,
self.expand_dis)
if self.check_collision(final_node, self.obstacle_list):
return self.generate_final_course(len(self.node_list) - 1)
@@ -108,6 +117,8 @@ class RRT:
if d <= self.path_resolution:
new_node.path_x.append(to_node.x)
new_node.path_y.append(to_node.y)
new_node.x = to_node.x
new_node.y = to_node.y
new_node.parent = from_node
@@ -130,8 +141,9 @@ class RRT:
def get_random_node(self):
if random.randint(0, 100) > self.goal_sample_rate:
rnd = self.Node(random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand))
rnd = self.Node(
random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand))
else: # goal point sampling
rnd = self.Node(self.end.x, self.end.y)
return rnd
@@ -139,8 +151,9 @@ class RRT:
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])
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 in self.node_list:
@@ -167,8 +180,8 @@ class RRT:
@staticmethod
def get_nearest_node_index(node_list, rnd_node):
dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y)
** 2 for node in node_list]
dlist = [(node.x - rnd_node.x)**2 + (node.y - rnd_node.y)**2
for node in node_list]
minind = dlist.index(min(dlist))
return minind
@@ -184,7 +197,7 @@ class RRT:
dy_list = [oy - y for y in node.path_y]
d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]
if min(d_list) <= size ** 2:
if min(d_list) <= size**2:
return False # collision
return True # safe
@@ -202,20 +215,14 @@ def main(gx=6.0, gy=10.0):
print("start " + __file__)
# ====Search Path with RRT====
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]
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]
# Set Initial parameters
rrt = RRT(start=[0, 0],
goal=[gx, gy],
rand_area=[-2, 15],
obstacle_list=obstacleList)
rrt = RRT(
start=[0, 0],
goal=[gx, gy],
rand_area=[-2, 15],
obstacle_list=obstacleList)
path = rrt.planning(animation=show_animation)
if path is None:

132
PathPlanning/RRTStar/rrt_star.py
View File

@@ -12,8 +12,7 @@ import sys
import matplotlib.pyplot as plt
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../RRT/")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../RRT/")
try:
from rrt import RRT
@@ -33,15 +32,17 @@ class RRTStar(RRT):
super().__init__(x, y)
self.cost = 0.0
def __init__(self, start, goal, obstacle_list, rand_area,
def __init__(self,
start,
goal,
obstacle_list,
rand_area,
expand_dis=30.0,
path_resolution=1.0,
goal_sample_rate=20,
max_iter=300,
connect_circle_dist=50.0
):
super().__init__(start, goal, obstacle_list,
rand_area, expand_dis, path_resolution, goal_sample_rate, max_iter)
connect_circle_dist=50.0,
search_until_max_iter=False):
"""
Setting Parameter
@@ -51,15 +52,17 @@ class RRTStar(RRT):
randArea:Random Sampling Area [min,max]
"""
super().__init__(start, goal, obstacle_list, rand_area, expand_dis,
path_resolution, goal_sample_rate, max_iter)
self.connect_circle_dist = connect_circle_dist
self.goal_node = self.Node(goal[0], goal[1])
self.search_until_max_iter = search_until_max_iter
def planning(self, animation=True, search_until_max_iter=True):
def planning(self, animation=True):
"""
rrt star path planning
animation: flag for animation on or off
search_until_max_iter: search until max iteration for path improving or not
animation: flag for animation on or off .
"""
self.node_list = [self.start]
@@ -67,19 +70,28 @@ class RRTStar(RRT):
print("Iter:", i, ", number of nodes:", len(self.node_list))
rnd = self.get_random_node()
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
new_node = self.steer(self.node_list[nearest_ind], rnd, self.expand_dis)
new_node = self.steer(self.node_list[nearest_ind], rnd,
self.expand_dis)
near_node = self.node_list[nearest_ind]
new_node.cost = near_node.cost + \
math.hypot(new_node.x-near_node.x,
new_node.y-near_node.y)
if self.check_collision(new_node, self.obstacle_list):
near_inds = self.find_near_nodes(new_node)
new_node = self.choose_parent(new_node, near_inds)
if new_node:
node_with_updated_parent = self.choose_parent(
new_node, near_inds)
if node_with_updated_parent:
self.rewire(node_with_updated_parent, near_inds)
self.node_list.append(node_with_updated_parent)
else:
self.node_list.append(new_node)
self.rewire(new_node, near_inds)
if animation and i % 5 == 0:
if animation:
self.draw_graph(rnd)
if (not search_until_max_iter) and new_node: # check reaching the goal
if ((not self.search_until_max_iter)
and new_node): # if reaches goal
last_index = self.search_best_goal_node()
if last_index is not None:
return self.generate_final_course(last_index)
@@ -93,6 +105,21 @@ class RRTStar(RRT):
return None
def choose_parent(self, new_node, near_inds):
"""
Computes the cheapest point to new_node contained in the list
near_inds and set such a node as the parent of new_node.
Arguments:
--------
new_node, Node
randomly generated node with a path from its neared point
There are not coalitions between this node and th tree.
near_inds: list
Indices of indices of the nodes what are near to new_node
Returns.
------
Node, a copy of new_node
"""
if not near_inds:
return None
@@ -113,14 +140,18 @@ class RRTStar(RRT):
min_ind = near_inds[costs.index(min_cost)]
new_node = self.steer(self.node_list[min_ind], new_node)
new_node.parent = self.node_list[min_ind]
new_node.cost = min_cost
return new_node
def search_best_goal_node(self):
dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) 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_dis]
dist_to_goal_list = [
self.calc_dist_to_goal(n.x, n.y) 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_dis
]
safe_goal_inds = []
for goal_ind in goal_inds:
@@ -139,17 +170,48 @@ class RRTStar(RRT):
return None
def find_near_nodes(self, new_node):
"""
1) defines a ball centered on new_node
2) Returns all nodes of the three that are inside this ball
Arguments:
---------
new_node: Node
new randomly generated node, without collisions between
its nearest node
Returns:
-------
list
List with the indices of the nodes inside the ball of
radius r
"""
nnode = len(self.node_list) + 1
r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))
# if expand_dist exists, search vertices in a range no more than expand_dist
if hasattr(self, 'expand_dis'):
# if expand_dist exists, search vertices in a range no more than
# expand_dist
if hasattr(self, 'expand_dis'):
r = min(r, self.expand_dis)
dist_list = [(node.x - new_node.x) ** 2 +
(node.y - new_node.y) ** 2 for node in self.node_list]
near_inds = [dist_list.index(i) for i in dist_list if i <= r ** 2]
dist_list = [(node.x - new_node.x)**2 + (node.y - new_node.y)**2
for node in self.node_list]
near_inds = [dist_list.index(i) for i in dist_list if i <= r**2]
return near_inds
def rewire(self, new_node, near_inds):
"""
For each node in near_inds, this will check if it is cheaper to
arrive to them from new_node.
In such a case, this will re-assign the parent of the nodes in
near_inds to new_node.
Parameters:
----------
new_node, Node
Node randomly added which can be joined to the tree
near_inds, list of uints
A list of indices of the self.new_node which contains
nodes within a circle of a given radius.
Remark: parent is designated in choose_parent.
"""
for i in near_inds:
near_node = self.node_list[i]
edge_node = self.steer(new_node, near_node)
@@ -161,7 +223,12 @@ class RRTStar(RRT):
improved_cost = near_node.cost > edge_node.cost
if no_collision and improved_cost:
self.node_list[i] = edge_node
near_node.x = edge_node.x
near_node.y = edge_node.y
near_node.cost = edge_node.cost
near_node.path_x = edge_node.path_x
near_node.path_y = edge_node.path_y
near_node.parent = edge_node.parent
self.propagate_cost_to_leaves(new_node)
def calc_new_cost(self, from_node, to_node):
@@ -192,10 +259,12 @@ def main():
] # [x,y,size(radius)]
# Set Initial parameters
rrt_star = RRTStar(start=[0, 0],
goal=[6, 10],
rand_area=[-2, 15],
obstacle_list=obstacle_list)
rrt_star = RRTStar(
start=[0, 0],
goal=[6, 10],
rand_area=[-2, 15],
obstacle_list=obstacle_list,
expand_dis=1)
path = rrt_star.planning(animation=show_animation)
if path is None:
@@ -206,10 +275,9 @@ def main():
# Draw final path
if show_animation:
rrt_star.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.plot([x for (x, y) in path], [y for (x, y) in path], 'r--')
plt.grid(True)
plt.pause(0.01) # Need for Mac
plt.show()
plt.show()
if __name__ == '__main__':