From db832a65c9a52a863f1725d28a592c4e19aa8a44 Mon Sep 17 00:00:00 2001 From: AtsushiSakai Date: Sun, 27 Mar 2016 17:08:00 +0900 Subject: [PATCH] add new sample --- .../RRT/rrt_with_pathsmoothing.py | 277 ++++++++++++++++++ scripts/PathPlanning/RRT/simple_rrt.py | 1 + 2 files changed, 278 insertions(+) create mode 100644 scripts/PathPlanning/RRT/rrt_with_pathsmoothing.py diff --git a/scripts/PathPlanning/RRT/rrt_with_pathsmoothing.py b/scripts/PathPlanning/RRT/rrt_with_pathsmoothing.py new file mode 100644 index 00000000..8031c6d3 --- /dev/null +++ b/scripts/PathPlanning/RRT/rrt_with_pathsmoothing.py @@ -0,0 +1,277 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +u""" +@brief: Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT) + +@author: AtsushiSakai + +@license: MIT + +""" + +import random +import math +import copy + +class RRT(): + u""" + Class for RRT Planning + """ + + def __init__(self, start, goal, obstacleList,randArea,expandDis=1.0,goalSampleRate=5,maxIter=500): + u""" + 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 + + def Planning(self,animation=True): + u""" + 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, 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): + import matplotlib.pyplot as plt + 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 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=range(0,360,5) + deg.append(0) + xl=[x+size*math.cos(math.radians(d)) for d in deg] + yl=[y+size*math.sin(math.radians(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(): + u""" + RRT Node + """ + + def __init__(self, x, y): + self.x = x + self.y = y + self.parent = None + + +def GetPathLength(path): + l = 0 + for i in range(len(path) - 1): + dx = path[i + 1][0] - path[i][0] + dy = path[i + 1][1] - path[i][1] + d = math.sqrt(dx * dx + dy * dy) + l += d + + return l + + +def GetTargetPoint(path, targetL): + l = 0 + ti = 0 + lastPairLen = 0 + for i in range(len(path) - 1): + dx = path[i + 1][0] - path[i][0] + dy = path[i + 1][1] - path[i][1] + d = math.sqrt(dx * dx + dy * dy) + l += d + if l >= targetL: + ti = i-1 + lastPairLen = d + break + + partRatio = (l - 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): + # Line Equation + + x1=first[0] + y1=first[1] + x2=second[0] + y2=second[1] + + try: + a=y2-y1 + b=-(x2-x1) + c=y2*(x2-x1)-x2*(y2-y1) + except ZeroDivisionError: + return False + + # print(first) + # print(second) + + for (ox,oy,size) in obstacleList: + d=abs(a*ox+b*oy+c)/(math.sqrt(a*a+b*b)) + # print((ox,oy,size,d)) + if d<=(size): + # print("NG") + return False + + # print("OK") + + return True # OK + + +def PathSmoothing(path, maxIter, obstacleList): + # print("PathSmoothing") + + l = GetPathLength(path) + + for i in range(maxIter): + # Sample two points + pickPoints = [random.uniform(0, l), random.uniform(0, l)] + pickPoints.sort() + # print(pickPoints) + first = GetTargetPoint(path, pickPoints[0]) + # print(first) + second = GetTargetPoint(path, pickPoints[1]) + # print(second) + + if first[2]<=0 or second[2]<=0: + continue + + if (second[2]+1) > len(path): + continue + + if second[2]==first[2]: + continue + + # collision check + if not LineCollisionCheck(first, second, obstacleList): + continue + + #Create New path + newPath=[] + newPath.extend(path[:first[2]+1]) + newPath.append([first[0],first[1]]) + newPath.append([second[0],second[1]]) + newPath.extend(path[second[2]+1:]) + path=newPath + l = GetPathLength(path) + + return path + + +if __name__ == '__main__': + import matplotlib.pyplot as plt + #====Search Path with RRT==== + # Parameter + obstacleList = [ + (5, 5, 1), + (3, 6, 2), + (3, 8, 2), + (3, 10, 2), + (7, 5, 2), + (9, 5, 2) + ] # [x,y,size] + rrt=RRT(start=[0,0],goal=[5,10],randArea=[-2,15],obstacleList=obstacleList) + path=rrt.Planning(animation=True) + + # Draw final path + rrt.DrawGraph() + plt.plot([x for (x,y) in path], [y for (x,y) in path],'-r') + + #Path smoothing + maxIter=1000 + smoothedPath = PathSmoothing(path, maxIter, obstacleList) + plt.plot([x for (x,y) in smoothedPath], [y for (x,y) in smoothedPath],'-b') + + plt.grid(True) + plt.pause(0.01) # Need for Mac + plt.show() diff --git a/scripts/PathPlanning/RRT/simple_rrt.py b/scripts/PathPlanning/RRT/simple_rrt.py index 082525c0..28947736 100644 --- a/scripts/PathPlanning/RRT/simple_rrt.py +++ b/scripts/PathPlanning/RRT/simple_rrt.py @@ -9,6 +9,7 @@ u""" """ + import random import math import copy