mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:41 -04:00
clean rrt codes
This commit is contained in:
Atsushi Sakai
198
PathPlanning/RRT/rrt.py
Normal file
198
PathPlanning/RRT/rrt.py
Normal file
@@ -0,0 +1,198 @@
|
||||
"""
|
||||
|
||||
Path planning Sample Code with Randomized Rapidly-Exploring Random Trees (RRT)
|
||||
|
||||
author: AtsushiSakai(@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import math
|
||||
import random
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
class RRT:
|
||||
"""
|
||||
Class for RRT planning
|
||||
"""
|
||||
|
||||
class Node():
|
||||
"""
|
||||
RRT Node
|
||||
"""
|
||||
|
||||
def __init__(self, x, y):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.parent = None
|
||||
|
||||
def __init__(self, start, goal, obstacle_list,
|
||||
rand_area, expand_dis=1.0, goal_sample_rate=5, max_iter=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 = 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.expand_dis = expand_dis
|
||||
self.goal_sample_rate = goal_sample_rate
|
||||
self.max_iter = max_iter
|
||||
self.obstacleList = obstacle_list
|
||||
self.node_list = []
|
||||
|
||||
def planning(self, animation=True):
|
||||
"""
|
||||
rrt path planning
|
||||
|
||||
animation: flag for animation on or off
|
||||
"""
|
||||
|
||||
self.node_list = [self.start]
|
||||
for i in range(self.max_iter):
|
||||
rnd = self.get_random_point()
|
||||
nearest_ind = self.get_nearest_list_index(self.node_list, rnd)
|
||||
nearest_node = self.node_list[nearest_ind]
|
||||
|
||||
new_node = self.steer(rnd, nearest_node)
|
||||
new_node.parent = nearest_node
|
||||
|
||||
if not self.check_collision(new_node, self.obstacleList):
|
||||
continue
|
||||
|
||||
self.node_list.append(new_node)
|
||||
print("nNodelist:", len(self.node_list))
|
||||
|
||||
# check goal
|
||||
if self.calc_dist_to_goal(new_node.x, new_node.y) <= self.expand_dis:
|
||||
print("Goal!!")
|
||||
return self.generate_final_course(len(self.node_list) - 1)
|
||||
|
||||
if animation and i % 5:
|
||||
self.draw_graph(rnd)
|
||||
|
||||
return None # cannot find path
|
||||
|
||||
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.expand_dis:
|
||||
new_node.x = nearest_node.x + self.expand_dis * math.cos(theta)
|
||||
new_node.y = nearest_node.y + self.expand_dis * math.sin(theta)
|
||||
|
||||
return new_node
|
||||
|
||||
def generate_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):
|
||||
dx = x - self.end.x
|
||||
dy = y - self.end.y
|
||||
return math.sqrt(dx ** 2 + dy ** 2)
|
||||
|
||||
def get_random_point(self):
|
||||
if random.randint(0, 100) > self.goal_sample_rate:
|
||||
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 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:
|
||||
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(gx=5.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)
|
||||
] # [x,y,size]
|
||||
# Set Initial parameters
|
||||
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:
|
||||
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,152 +1,22 @@
|
||||
"""
|
||||
Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT)
|
||||
|
||||
Path planning Sample Code with RRT with path smoothing
|
||||
|
||||
@author: AtsushiSakai(@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import random
|
||||
import math
|
||||
import copy
|
||||
import numpy as np
|
||||
import random
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
from rrt import RRT
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
class RRT():
|
||||
"""
|
||||
Class for RRT Planning
|
||||
"""
|
||||
|
||||
def __init__(self, start, goal, obstacleList, randArea, expandDis=1.0, goalSampleRate=5, 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):
|
||||
"""
|
||||
Pathplanning
|
||||
|
||||
animation: flag for animation on or off
|
||||
"""
|
||||
|
||||
self.nodeList = [self.start]
|
||||
while True:
|
||||
# Random Sampling
|
||||
if random.randint(0, 100) > self.goalSampleRate:
|
||||
rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(
|
||||
self.minrand, self.maxrand)]
|
||||
else:
|
||||
rnd = [self.end.x, self.end.y]
|
||||
|
||||
# Find nearest node
|
||||
nind = self.GetNearestListIndex(self.nodeList, rnd)
|
||||
# print(nind)
|
||||
|
||||
# expand tree
|
||||
nearestNode = self.nodeList[nind]
|
||||
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
|
||||
|
||||
newNode = copy.deepcopy(nearestNode)
|
||||
newNode.x += self.expandDis * math.cos(theta)
|
||||
newNode.y += self.expandDis * math.sin(theta)
|
||||
newNode.parent = nind
|
||||
|
||||
if not self.__CollisionCheck(newNode, self.obstacleList):
|
||||
continue
|
||||
|
||||
self.nodeList.append(newNode)
|
||||
|
||||
# check goal
|
||||
dx = newNode.x - self.end.x
|
||||
dy = newNode.y - self.end.y
|
||||
d = math.sqrt(dx * dx + dy * dy)
|
||||
if d <= self.expandDis:
|
||||
print("Goal!!")
|
||||
break
|
||||
|
||||
if animation:
|
||||
self.DrawGraph(rnd)
|
||||
|
||||
path = [[self.end.x, self.end.y]]
|
||||
lastIndex = len(self.nodeList) - 1
|
||||
while self.nodeList[lastIndex].parent is not None:
|
||||
node = self.nodeList[lastIndex]
|
||||
path.append([node.x, node.y])
|
||||
lastIndex = node.parent
|
||||
path.append([self.start.x, self.start.y])
|
||||
|
||||
return path
|
||||
|
||||
def DrawGraph(self, rnd=None):
|
||||
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 (x, y, size) in self.obstacleList:
|
||||
self.PlotCircle(x, y, 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 PlotCircle(self, x, y, size):
|
||||
deg = list(range(0, 360, 5))
|
||||
deg.append(0)
|
||||
xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
|
||||
yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
|
||||
plt.plot(xl, yl, "-k")
|
||||
|
||||
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 = math.sqrt(dx * dx + dy * dy)
|
||||
if d <= size:
|
||||
return False # collision
|
||||
|
||||
return True # safe
|
||||
|
||||
|
||||
class Node():
|
||||
"""
|
||||
RRT Node
|
||||
"""
|
||||
|
||||
def __init__(self, x, y):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.parent = None
|
||||
|
||||
|
||||
def GetPathLength(path):
|
||||
def get_path_length(path):
|
||||
le = 0
|
||||
for i in range(len(path) - 1):
|
||||
dx = path[i + 1][0] - path[i][0]
|
||||
@@ -157,7 +27,7 @@ def GetPathLength(path):
|
||||
return le
|
||||
|
||||
|
||||
def GetTargetPoint(path, targetL):
|
||||
def get_target_point(path, targetL):
|
||||
le = 0
|
||||
ti = 0
|
||||
lastPairLen = 0
|
||||
@@ -172,17 +42,14 @@ def GetTargetPoint(path, targetL):
|
||||
break
|
||||
|
||||
partRatio = (le - targetL) / lastPairLen
|
||||
# print(partRatio)
|
||||
# print((ti,len(path),path[ti],path[ti+1]))
|
||||
|
||||
x = path[ti][0] + (path[ti + 1][0] - path[ti][0]) * partRatio
|
||||
y = path[ti][1] + (path[ti + 1][1] - path[ti][1]) * partRatio
|
||||
# print((x,y))
|
||||
|
||||
return [x, y, ti]
|
||||
|
||||
|
||||
def LineCollisionCheck(first, second, obstacleList):
|
||||
def line_collision_check(first, second, obstacleList):
|
||||
# Line Equation
|
||||
|
||||
x1 = first[0]
|
||||
@@ -199,7 +66,7 @@ def LineCollisionCheck(first, second, obstacleList):
|
||||
|
||||
for (ox, oy, size) in obstacleList:
|
||||
d = abs(a * ox + b * oy + c) / (math.sqrt(a * a + b * b))
|
||||
if d <= (size):
|
||||
if d <= size:
|
||||
return False
|
||||
|
||||
# print("OK")
|
||||
@@ -207,20 +74,15 @@ def LineCollisionCheck(first, second, obstacleList):
|
||||
return True # OK
|
||||
|
||||
|
||||
def PathSmoothing(path, maxIter, obstacleList):
|
||||
# print("PathSmoothing")
|
||||
def path_smoothing(path, max_iter, obstacle_list):
|
||||
le = get_path_length(path)
|
||||
|
||||
le = GetPathLength(path)
|
||||
|
||||
for i in range(maxIter):
|
||||
for i in range(max_iter):
|
||||
# Sample two points
|
||||
pickPoints = [random.uniform(0, le), random.uniform(0, le)]
|
||||
pickPoints.sort()
|
||||
# print(pickPoints)
|
||||
first = GetTargetPoint(path, pickPoints[0])
|
||||
# print(first)
|
||||
second = GetTargetPoint(path, pickPoints[1])
|
||||
# print(second)
|
||||
first = get_target_point(path, pickPoints[0])
|
||||
second = get_target_point(path, pickPoints[1])
|
||||
|
||||
if first[2] <= 0 or second[2] <= 0:
|
||||
continue
|
||||
@@ -232,7 +94,7 @@ def PathSmoothing(path, maxIter, obstacleList):
|
||||
continue
|
||||
|
||||
# collision check
|
||||
if not LineCollisionCheck(first, second, obstacleList):
|
||||
if not line_collision_check(first, second, obstacle_list):
|
||||
continue
|
||||
|
||||
# Create New path
|
||||
@@ -242,7 +104,7 @@ def PathSmoothing(path, maxIter, obstacleList):
|
||||
newPath.append([second[0], second[1]])
|
||||
newPath.extend(path[second[2] + 1:])
|
||||
path = newPath
|
||||
le = GetPathLength(path)
|
||||
le = get_path_length(path)
|
||||
|
||||
return path
|
||||
|
||||
@@ -259,16 +121,16 @@ def main():
|
||||
(9, 5, 2)
|
||||
] # [x,y,size]
|
||||
rrt = RRT(start=[0, 0], goal=[5, 10],
|
||||
randArea=[-2, 15], obstacleList=obstacleList)
|
||||
path = rrt.Planning(animation=show_animation)
|
||||
rand_area=[-2, 15], obstacle_list=obstacleList)
|
||||
path = rrt.planning(animation=show_animation)
|
||||
|
||||
# Path smoothing
|
||||
maxIter = 1000
|
||||
smoothedPath = PathSmoothing(path, maxIter, obstacleList)
|
||||
smoothedPath = path_smoothing(path, maxIter, obstacleList)
|
||||
|
||||
# Draw final path
|
||||
if show_animation:
|
||||
rrt.DrawGraph()
|
||||
rrt.draw_graph()
|
||||
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
|
||||
|
||||
plt.plot([x for (x, y) in smoothedPath], [
|
||||
|
||||
@@ -1,173 +0,0 @@
|
||||
"""
|
||||
Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT)
|
||||
|
||||
author: AtsushiSakai(@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import random
|
||||
import math
|
||||
import copy
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
class RRT():
|
||||
"""
|
||||
Class for RRT Planning
|
||||
"""
|
||||
|
||||
def __init__(self, start, goal, obstacleList,
|
||||
randArea, expandDis=1.0, goalSampleRate=5, 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):
|
||||
"""
|
||||
Pathplanning
|
||||
|
||||
animation: flag for animation on or off
|
||||
"""
|
||||
|
||||
self.nodeList = [self.start]
|
||||
while True:
|
||||
# Random Sampling
|
||||
if random.randint(0, 100) > self.goalSampleRate:
|
||||
rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(
|
||||
self.minrand, self.maxrand)]
|
||||
else:
|
||||
rnd = [self.end.x, self.end.y]
|
||||
|
||||
# Find nearest node
|
||||
nind = self.GetNearestListIndex(self.nodeList, rnd)
|
||||
# print(nind)
|
||||
|
||||
# expand tree
|
||||
nearestNode = self.nodeList[nind]
|
||||
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
|
||||
|
||||
newNode = copy.deepcopy(nearestNode)
|
||||
newNode.x += self.expandDis * math.cos(theta)
|
||||
newNode.y += self.expandDis * math.sin(theta)
|
||||
newNode.parent = nind
|
||||
|
||||
if not self.__CollisionCheck(newNode, self.obstacleList):
|
||||
continue
|
||||
|
||||
self.nodeList.append(newNode)
|
||||
print("nNodelist:", len(self.nodeList))
|
||||
|
||||
# check goal
|
||||
dx = newNode.x - self.end.x
|
||||
dy = newNode.y - self.end.y
|
||||
d = math.sqrt(dx * dx + dy * dy)
|
||||
if d <= self.expandDis:
|
||||
print("Goal!!")
|
||||
break
|
||||
|
||||
if animation:
|
||||
self.DrawGraph(rnd)
|
||||
|
||||
path = [[self.end.x, self.end.y]]
|
||||
lastIndex = len(self.nodeList) - 1
|
||||
while self.nodeList[lastIndex].parent is not None:
|
||||
node = self.nodeList[lastIndex]
|
||||
path.append([node.x, node.y])
|
||||
lastIndex = node.parent
|
||||
path.append([self.start.x, self.start.y])
|
||||
|
||||
return path
|
||||
|
||||
def DrawGraph(self, rnd=None): # pragma: no cover
|
||||
"""
|
||||
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 = math.sqrt(dx * dx + dy * dy)
|
||||
if d <= size:
|
||||
return False # collision
|
||||
|
||||
return True # safe
|
||||
|
||||
|
||||
class Node():
|
||||
"""
|
||||
RRT Node
|
||||
"""
|
||||
|
||||
def __init__(self, x, y):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.parent = None
|
||||
|
||||
|
||||
def main(gx=5.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)
|
||||
] # [x,y,size]
|
||||
# Set Initial parameters
|
||||
rrt = RRT(start=[0, 0], goal=[gx, gy],
|
||||
randArea=[-2, 15], obstacleList=obstacleList)
|
||||
path = rrt.Planning(animation=show_animation)
|
||||
|
||||
# Draw final path
|
||||
if show_animation: # pragma: no cover
|
||||
rrt.DrawGraph()
|
||||
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
|
||||
plt.grid(True)
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
74
PathPlanning/RRTStar/rrt_star.ipynb
Normal file
74
PathPlanning/RRTStar/rrt_star.ipynb
Normal file
File diff suppressed because one or more lines are too long
@@ -1,4 +1,5 @@
|
||||
"""
|
||||
|
||||
Path planning Sample Code with RRT*
|
||||
|
||||
author: Atsushi Sakai(@Atsushi_twi)
|
||||
@@ -7,21 +8,28 @@ author: Atsushi Sakai(@Atsushi_twi)
|
||||
|
||||
import copy
|
||||
import math
|
||||
import random
|
||||
import os
|
||||
import sys
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
||||
"/../RRT/")
|
||||
|
||||
try:
|
||||
from RRT.rrt import RRT
|
||||
except ImportError:
|
||||
raise
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
class RRTStar:
|
||||
class RRTStar(RRT):
|
||||
"""
|
||||
Class for RRT planning
|
||||
Class for RRT Star planning
|
||||
"""
|
||||
|
||||
class Node:
|
||||
|
||||
def __init__(self, x, y):
|
||||
self.x = x
|
||||
self.y = y
|
||||
@@ -34,6 +42,8 @@ class RRTStar:
|
||||
max_iter=500,
|
||||
connect_circle_dist=50.0
|
||||
):
|
||||
super().__init__(start, goal, obstacle_list,
|
||||
rand_area, expand_dis, goal_sample_rate, max_iter)
|
||||
"""
|
||||
Setting Parameter
|
||||
|
||||
@@ -44,29 +54,19 @@ class RRTStar:
|
||||
|
||||
"""
|
||||
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
|
||||
rrt star 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):
|
||||
for i in range(self.max_iter):
|
||||
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):
|
||||
@@ -74,21 +74,21 @@ class RRTStar:
|
||||
new_node = self.choose_parent(new_node, near_inds)
|
||||
if new_node:
|
||||
self.node_list.append(new_node)
|
||||
self.rewire(new_node, near_inds)
|
||||
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
|
||||
if not search_until_maxiter and new_node: # 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)
|
||||
if d <= self.expand_dis:
|
||||
return self.generate_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 self.generate_final_course(last_index)
|
||||
|
||||
return None
|
||||
|
||||
@@ -116,28 +116,9 @@ class RRTStar:
|
||||
|
||||
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]
|
||||
goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.expand_dis]
|
||||
|
||||
if not goal_inds:
|
||||
return None
|
||||
@@ -149,19 +130,6 @@ class RRTStar:
|
||||
|
||||
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))
|
||||
@@ -193,60 +161,14 @@ class RRTStar:
|
||||
|
||||
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)
|
||||
for i in range(int(d / self.expand_dis)):
|
||||
tmp_node.x += self.expand_dis * math.cos(theta)
|
||||
tmp_node.y += self.expand_dis * 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__)
|
||||
|
||||
@@ -1,12 +1,12 @@
|
||||
import os
|
||||
import sys
|
||||
from unittest import TestCase
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
|
||||
try:
|
||||
from PathPlanning.RRT import simple_rrt as m
|
||||
from PathPlanning.RRT import rrt as m
|
||||
from PathPlanning.RRT import rrt_with_pathsmoothing as m1
|
||||
except:
|
||||
except ImportError:
|
||||
raise
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user