mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-10 05:28:07 -05:00
Fix RRT Star algorithm
This commit is contained in:
Atsushi Sakai
2
.gitignore
vendored
2
.gitignore
vendored
@@ -68,3 +68,5 @@ target/
|
||||
|
||||
#Ipython Notebook
|
||||
.ipynb_checkpoints
|
||||
|
||||
matplotrecorder/*
|
||||
|
||||
286
PathPlanning/RRTStar/rrt_star.py
Normal file
286
PathPlanning/RRTStar/rrt_star.py
Normal file
@@ -0,0 +1,286 @@
|
||||
"""
|
||||
Path planning Sample Code with RRT*
|
||||
|
||||
author: Atsushi Sakai(@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import copy
|
||||
import math
|
||||
import random
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
class RRTStar:
|
||||
"""
|
||||
Class for RRT planning
|
||||
"""
|
||||
|
||||
class Node:
|
||||
|
||||
def __init__(self, x, y):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.cost = 0.0
|
||||
self.parent = None
|
||||
|
||||
def __init__(self, start, goal, obstacle_list, rand_area,
|
||||
expand_dis=0.5,
|
||||
goal_sample_rate=20,
|
||||
max_iter=500,
|
||||
connect_circle_dist=50.0
|
||||
):
|
||||
"""
|
||||
Setting Parameter
|
||||
|
||||
start:Start Position [x,y]
|
||||
goal:Goal Position [x,y]
|
||||
obstacleList:obstacle Positions [[x,y,size],...]
|
||||
randArea:Random Sampling Area [min,max]
|
||||
|
||||
"""
|
||||
self.connect_circle_dist = connect_circle_dist
|
||||
self.start = self.Node(start[0], start[1])
|
||||
self.end = self.Node(goal[0], goal[1])
|
||||
self.min_rand = rand_area[0]
|
||||
self.max_rand = rand_area[1]
|
||||
self.expandDis = expand_dis
|
||||
self.goalSampleRate = goal_sample_rate
|
||||
self.maxIter = max_iter
|
||||
self.obstacleList = obstacle_list
|
||||
self.node_list = []
|
||||
|
||||
def planning(self, animation=True, search_until_maxiter=True):
|
||||
"""
|
||||
rrt path planning
|
||||
|
||||
animation: flag for animation on or off
|
||||
search_until_maxiter: search until max iteration for path improving or not
|
||||
"""
|
||||
|
||||
self.node_list = [self.start]
|
||||
for i in range(self.maxIter):
|
||||
rnd = self.get_random_point()
|
||||
nearest_ind = self.get_nearest_list_index(self.node_list, rnd)
|
||||
|
||||
new_node = self.steer(rnd, self.node_list[nearest_ind])
|
||||
|
||||
if self.check_collision(new_node, self.obstacleList):
|
||||
near_inds = self.find_near_nodes(new_node)
|
||||
new_node = self.choose_parent(new_node, near_inds)
|
||||
if new_node:
|
||||
self.node_list.append(new_node)
|
||||
self.rewire(new_node, near_inds)
|
||||
|
||||
if animation and i % 5 == 0:
|
||||
self.draw_graph(rnd)
|
||||
|
||||
if not search_until_maxiter: # check reaching the goal
|
||||
d, _ = self.calc_distance_and_angle(new_node, self.end)
|
||||
if d <= self.expandDis:
|
||||
return self.gen_final_course(len(self.node_list) - 1)
|
||||
|
||||
print("reached max iteration")
|
||||
|
||||
last_index = self.search_best_goal_node()
|
||||
if last_index:
|
||||
return self.gen_final_course(last_index)
|
||||
|
||||
return None
|
||||
|
||||
def choose_parent(self, new_node, near_inds):
|
||||
if not near_inds:
|
||||
return None
|
||||
|
||||
# search nearest cost in near_inds
|
||||
costs = []
|
||||
for i in near_inds:
|
||||
d, theta = self.calc_distance_and_angle(self.node_list[i], new_node)
|
||||
if self.check_collision_extend(self.node_list[i], theta, d):
|
||||
costs.append(self.node_list[i].cost + d)
|
||||
else:
|
||||
costs.append(float("inf")) # the cost of collision node
|
||||
min_cost = min(costs)
|
||||
|
||||
if min_cost == float("inf"):
|
||||
print("There is no good path.(min_cost is inf)")
|
||||
return None
|
||||
|
||||
new_node.cost = min_cost
|
||||
min_ind = near_inds[costs.index(min_cost)]
|
||||
new_node.parent = self.node_list[min_ind]
|
||||
|
||||
return new_node
|
||||
|
||||
def steer(self, rnd, nearest_node):
|
||||
new_node = self.Node(rnd[0], rnd[1])
|
||||
d, theta = self.calc_distance_and_angle(nearest_node, new_node)
|
||||
if d > self.expandDis:
|
||||
new_node.x = nearest_node.x + self.expandDis * math.cos(theta)
|
||||
new_node.y = nearest_node.y + self.expandDis * math.sin(theta)
|
||||
new_node.cost = float("inf")
|
||||
return new_node
|
||||
|
||||
def get_random_point(self):
|
||||
|
||||
if random.randint(0, 100) > self.goalSampleRate:
|
||||
rnd = [random.uniform(self.min_rand, self.max_rand),
|
||||
random.uniform(self.min_rand, self.max_rand)]
|
||||
else: # goal point sampling
|
||||
rnd = [self.end.x, self.end.y]
|
||||
|
||||
return rnd
|
||||
|
||||
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.expandDis]
|
||||
|
||||
if not goal_inds:
|
||||
return None
|
||||
|
||||
min_cost = min([self.node_list[i].cost for i in goal_inds])
|
||||
for i in goal_inds:
|
||||
if self.node_list[i].cost == min_cost:
|
||||
return i
|
||||
|
||||
return None
|
||||
|
||||
def gen_final_course(self, goal_ind):
|
||||
path = [[self.end.x, self.end.y]]
|
||||
node = self.node_list[goal_ind]
|
||||
while node.parent is not None:
|
||||
path.append([node.x, node.y])
|
||||
node = node.parent
|
||||
path.append([node.x, node.y])
|
||||
|
||||
return path
|
||||
|
||||
def calc_dist_to_goal(self, x, y):
|
||||
return np.linalg.norm([x - self.end.x, y - self.end.y])
|
||||
|
||||
def find_near_nodes(self, new_node):
|
||||
nnode = len(self.node_list) + 1
|
||||
r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))
|
||||
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 i in near_inds:
|
||||
near_node = self.node_list[i]
|
||||
d, theta = self.calc_distance_and_angle(near_node, new_node)
|
||||
new_cost = new_node.cost + d
|
||||
|
||||
if near_node.cost > new_cost:
|
||||
if self.check_collision_extend(near_node, theta, d):
|
||||
near_node.parent = new_node
|
||||
near_node.cost = new_cost
|
||||
self.propagate_cost_to_leaves(new_node)
|
||||
|
||||
def propagate_cost_to_leaves(self, parent_node):
|
||||
for node in self.node_list:
|
||||
if node.parent == parent_node:
|
||||
d, _ = self.calc_distance_and_angle(parent_node, node)
|
||||
node.cost = parent_node.cost + d
|
||||
self.propagate_cost_to_leaves(node)
|
||||
|
||||
def check_collision_extend(self, near_node, theta, d):
|
||||
|
||||
tmp_node = copy.deepcopy(near_node)
|
||||
|
||||
for i in range(int(d / self.expandDis)):
|
||||
tmp_node.x += self.expandDis * math.cos(theta)
|
||||
tmp_node.y += self.expandDis * math.sin(theta)
|
||||
if not self.check_collision(tmp_node, self.obstacleList):
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def draw_graph(self, rnd=None):
|
||||
plt.clf()
|
||||
if rnd is not None:
|
||||
plt.plot(rnd[0], rnd[1], "^k")
|
||||
for node in self.node_list:
|
||||
if node.parent is not None:
|
||||
plt.plot([node.x, node.parent.x],
|
||||
[node.y, node.parent.y],
|
||||
"-g")
|
||||
|
||||
for (ox, oy, size) in self.obstacleList:
|
||||
plt.plot(ox, oy, "ok", ms=30 * size)
|
||||
|
||||
plt.plot(self.start.x, self.start.y, "xr")
|
||||
plt.plot(self.end.x, self.end.y, "xr")
|
||||
plt.axis([-2, 15, -2, 15])
|
||||
plt.grid(True)
|
||||
plt.pause(0.01)
|
||||
|
||||
@staticmethod
|
||||
def get_nearest_list_index(node_list, rnd):
|
||||
dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1])
|
||||
** 2 for node in node_list]
|
||||
minind = dlist.index(min(dlist))
|
||||
|
||||
return minind
|
||||
|
||||
@staticmethod
|
||||
def check_collision(node, obstacleList):
|
||||
for (ox, oy, size) in obstacleList:
|
||||
dx = ox - node.x
|
||||
dy = oy - node.y
|
||||
d = dx * dx + dy * dy
|
||||
if d <= size ** 2:
|
||||
return False # collision
|
||||
|
||||
return True # safe
|
||||
|
||||
@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.sqrt(dx ** 2 + dy ** 2)
|
||||
theta = math.atan2(dy, dx)
|
||||
return d, theta
|
||||
|
||||
|
||||
def main():
|
||||
print("Start " + __file__)
|
||||
|
||||
# ====Search Path with RRT====
|
||||
obstacle_list = [
|
||||
(5, 5, 1),
|
||||
(3, 6, 2),
|
||||
(3, 8, 2),
|
||||
(3, 10, 2),
|
||||
(7, 5, 2),
|
||||
(9, 5, 2)
|
||||
] # [x,y,size(radius)]
|
||||
|
||||
# Set Initial parameters
|
||||
rrt = RRTStar(start=[0, 0],
|
||||
goal=[10, 10],
|
||||
rand_area=[-2, 15],
|
||||
obstacle_list=obstacle_list)
|
||||
path = rrt.planning(animation=show_animation, search_until_maxiter=False)
|
||||
|
||||
if path is None:
|
||||
print("Cannot find path")
|
||||
else:
|
||||
print("found path!!")
|
||||
|
||||
# Draw final path
|
||||
if show_animation:
|
||||
rrt.draw_graph()
|
||||
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()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,285 +0,0 @@
|
||||
|
||||
|
||||
|
||||
"""
|
||||
Path Planning Sample Code with RRT*
|
||||
|
||||
author: AtsushiSakai(@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import random
|
||||
import math
|
||||
import copy
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
class Node():
|
||||
|
||||
def __init__(self, x, y):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.cost = 0.0
|
||||
self.parent = None
|
||||
|
||||
|
||||
class RRT():
|
||||
"""
|
||||
Class for RRT Planning
|
||||
"""
|
||||
|
||||
def __init__(self, start, goal, obstacleList, randArea,
|
||||
expandDis=0.5, goalSampleRate=20, maxIter=500):
|
||||
"""
|
||||
Setting Parameter
|
||||
|
||||
start:Start Position [x,y]
|
||||
goal:Goal Position [x,y]
|
||||
obstacleList:obstacle Positions [[x,y,size],...]
|
||||
randArea:Ramdom Samping Area [min,max]
|
||||
|
||||
"""
|
||||
self.start = Node(start[0], start[1])
|
||||
self.end = Node(goal[0], goal[1])
|
||||
self.minrand = randArea[0]
|
||||
self.maxrand = randArea[1]
|
||||
self.expandDis = expandDis
|
||||
self.goalSampleRate = goalSampleRate
|
||||
self.maxIter = maxIter
|
||||
self.obstacleList = obstacleList
|
||||
|
||||
def Planning(self, animation=True, search_until_maxiter=True):
|
||||
"""
|
||||
rrt path planning
|
||||
|
||||
animation: flag for animation on or off
|
||||
search_until_maxiter: search until max iteration for path improving or not
|
||||
"""
|
||||
|
||||
self.nodeList = [self.start]
|
||||
for i in range(self.maxIter):
|
||||
rnd = self.get_random_point()
|
||||
nind = self.GetNearestListIndex(self.nodeList, rnd)
|
||||
|
||||
new_node = self.steer(rnd, nind)
|
||||
|
||||
if self.__CollisionCheck(new_node, self.obstacleList):
|
||||
nearinds = self.find_near_nodes(new_node)
|
||||
new_node = self.choose_parent(new_node, nearinds)
|
||||
self.nodeList.append(new_node)
|
||||
self.rewire(new_node, nearinds)
|
||||
|
||||
if animation and i % 5 == 0:
|
||||
self.DrawGraph(rnd)
|
||||
|
||||
# generate course
|
||||
if not search_until_maxiter:
|
||||
lastIndex = self.get_best_last_index()
|
||||
if lastIndex:
|
||||
return self.gen_final_course(lastIndex)
|
||||
|
||||
print("reached max iteration")
|
||||
|
||||
lastIndex = self.get_best_last_index()
|
||||
if lastIndex:
|
||||
return self.gen_final_course(lastIndex)
|
||||
|
||||
return None
|
||||
|
||||
def choose_parent(self, new_node, nearinds):
|
||||
if not nearinds:
|
||||
return new_node
|
||||
|
||||
dlist = []
|
||||
for i in nearinds:
|
||||
dx = new_node.x - self.nodeList[i].x
|
||||
dy = new_node.y - self.nodeList[i].y
|
||||
d = math.sqrt(dx ** 2 + dy ** 2)
|
||||
theta = math.atan2(dy, dx)
|
||||
if self.check_collision_extend(self.nodeList[i], theta, d):
|
||||
dlist.append(self.nodeList[i].cost + d)
|
||||
else:
|
||||
dlist.append(float("inf"))
|
||||
|
||||
mincost = min(dlist)
|
||||
minind = nearinds[dlist.index(mincost)]
|
||||
|
||||
if mincost == float("inf"):
|
||||
print("mincost is inf")
|
||||
return new_node
|
||||
|
||||
new_node.cost = mincost
|
||||
new_node.parent = minind
|
||||
|
||||
return new_node
|
||||
|
||||
def steer(self, rnd, nind):
|
||||
|
||||
# expand tree
|
||||
nearest_node = self.nodeList[nind]
|
||||
theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x)
|
||||
new_node = Node(rnd[0], rnd[1])
|
||||
currentDistance = math.sqrt(
|
||||
(rnd[1] - nearest_node.y) ** 2 + (rnd[0] - nearest_node.x) ** 2)
|
||||
# Find a point within expandDis of nind, and closest to rnd
|
||||
if currentDistance <= self.expandDis:
|
||||
pass
|
||||
else:
|
||||
new_node.x = nearest_node.x + self.expandDis * math.cos(theta)
|
||||
new_node.y = nearest_node.y + self.expandDis * math.sin(theta)
|
||||
new_node.cost = float("inf")
|
||||
new_node.parent = None
|
||||
return new_node
|
||||
|
||||
def get_random_point(self):
|
||||
|
||||
if random.randint(0, 100) > self.goalSampleRate:
|
||||
rnd = [random.uniform(self.minrand, self.maxrand),
|
||||
random.uniform(self.minrand, self.maxrand)]
|
||||
else: # goal point sampling
|
||||
rnd = [self.end.x, self.end.y]
|
||||
|
||||
return rnd
|
||||
|
||||
def get_best_last_index(self):
|
||||
|
||||
disglist = [self.calc_dist_to_goal(
|
||||
node.x, node.y) for node in self.nodeList]
|
||||
goalinds = [disglist.index(i) for i in disglist if i <= self.expandDis]
|
||||
|
||||
if not goalinds:
|
||||
return None
|
||||
|
||||
mincost = min([self.nodeList[i].cost for i in goalinds])
|
||||
for i in goalinds:
|
||||
if self.nodeList[i].cost == mincost:
|
||||
return i
|
||||
|
||||
return None
|
||||
|
||||
def gen_final_course(self, goalind):
|
||||
path = [[self.end.x, self.end.y]]
|
||||
while self.nodeList[goalind].parent is not None:
|
||||
node = self.nodeList[goalind]
|
||||
path.append([node.x, node.y])
|
||||
goalind = node.parent
|
||||
path.append([self.start.x, self.start.y])
|
||||
return path
|
||||
|
||||
def calc_dist_to_goal(self, x, y):
|
||||
return np.linalg.norm([x - self.end.x, y - self.end.y])
|
||||
|
||||
def find_near_nodes(self, new_node):
|
||||
nnode = len(self.nodeList)
|
||||
r = 50.0 * math.sqrt((math.log(nnode) / nnode))
|
||||
dlist = [(node.x - new_node.x) ** 2 +
|
||||
(node.y - new_node.y) ** 2 for node in self.nodeList]
|
||||
nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
|
||||
return nearinds
|
||||
|
||||
def rewire(self, new_node, nearinds):
|
||||
nnode = len(self.nodeList)
|
||||
for i in nearinds:
|
||||
nearNode = self.nodeList[i]
|
||||
|
||||
dx = new_node.x - nearNode.x
|
||||
dy = new_node.y - nearNode.y
|
||||
d = math.sqrt(dx ** 2 + dy ** 2)
|
||||
|
||||
scost = new_node.cost + d
|
||||
|
||||
if nearNode.cost > scost:
|
||||
theta = math.atan2(dy, dx)
|
||||
if self.check_collision_extend(nearNode, theta, d):
|
||||
nearNode.parent = nnode - 1
|
||||
nearNode.cost = scost
|
||||
|
||||
def check_collision_extend(self, nearNode, theta, d):
|
||||
|
||||
tmpNode = copy.deepcopy(nearNode)
|
||||
|
||||
for i in range(int(d / self.expandDis)):
|
||||
tmpNode.x += self.expandDis * math.cos(theta)
|
||||
tmpNode.y += self.expandDis * math.sin(theta)
|
||||
if not self.__CollisionCheck(tmpNode, self.obstacleList):
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def DrawGraph(self, rnd=None):
|
||||
"""
|
||||
Draw Graph
|
||||
"""
|
||||
plt.clf()
|
||||
if rnd is not None:
|
||||
plt.plot(rnd[0], rnd[1], "^k")
|
||||
for node in self.nodeList:
|
||||
if node.parent is not None:
|
||||
plt.plot([node.x, self.nodeList[node.parent].x], [
|
||||
node.y, self.nodeList[node.parent].y], "-g")
|
||||
|
||||
for (ox, oy, size) in self.obstacleList:
|
||||
plt.plot(ox, oy, "ok", ms=30 * size)
|
||||
|
||||
plt.plot(self.start.x, self.start.y, "xr")
|
||||
plt.plot(self.end.x, self.end.y, "xr")
|
||||
plt.axis([-2, 15, -2, 15])
|
||||
plt.grid(True)
|
||||
plt.pause(0.01)
|
||||
|
||||
def GetNearestListIndex(self, nodeList, rnd):
|
||||
dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1])
|
||||
** 2 for node in nodeList]
|
||||
minind = dlist.index(min(dlist))
|
||||
|
||||
return minind
|
||||
|
||||
def __CollisionCheck(self, node, obstacleList):
|
||||
for (ox, oy, size) in obstacleList:
|
||||
dx = ox - node.x
|
||||
dy = oy - node.y
|
||||
d = dx * dx + dy * dy
|
||||
if d <= size ** 2:
|
||||
return False # collision
|
||||
|
||||
return True # safe
|
||||
|
||||
|
||||
def main():
|
||||
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)
|
||||
] # [x,y,size(radius)]
|
||||
|
||||
# Set Initial parameters
|
||||
rrt = RRT(start=[0, 0], goal=[10, 10],
|
||||
randArea=[-2, 15], obstacleList=obstacleList)
|
||||
path = rrt.Planning(animation=show_animation, search_until_maxiter=False)
|
||||
|
||||
if path is None:
|
||||
print("Cannot find path")
|
||||
else:
|
||||
print("found path!!")
|
||||
|
||||
# Draw final path
|
||||
if show_animation:
|
||||
rrt.DrawGraph()
|
||||
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()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
0
PathTracking/.gitignore
vendored
0
PathTracking/.gitignore
vendored
@@ -3,7 +3,7 @@ import sys
|
||||
from unittest import TestCase
|
||||
|
||||
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
||||
"/../PathPlanning/RRTstar/")
|
||||
"/../PathPlanning/RRTStar/")
|
||||
|
||||
try:
|
||||
import rrt_star as m
|
||||
|
||||
Reference in New Issue
Block a user