diff --git a/README.md b/README.md index b47f28a6..796902f5 100644 --- a/README.md +++ b/README.md @@ -23,6 +23,8 @@ see (in Japanese) : This script is a path planning code with RRT \* +- [Incremental Sampling-based Algorithms for Optimal Motion Planning](https://arxiv.org/abs/1005.0416) + ## Dubins path planning diff --git a/scripts/PathPlanning/RRT/figure_1.png b/scripts/PathPlanning/RRT/figure_1.png new file mode 100644 index 00000000..959a28b4 Binary files /dev/null and b/scripts/PathPlanning/RRT/figure_1.png differ diff --git a/scripts/PathPlanning/RRT/simple_rrt.py b/scripts/PathPlanning/RRT/simple_rrt.py index 28947736..2c5c9842 100644 --- a/scripts/PathPlanning/RRT/simple_rrt.py +++ b/scripts/PathPlanning/RRT/simple_rrt.py @@ -1,7 +1,7 @@ #!/usr/bin/python # -*- coding: utf-8 -*- u""" -@brief: Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT) +@brief: Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT) @author: AtsushiSakai @@ -14,12 +14,14 @@ 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): + def __init__(self, start, goal, obstacleList, + randArea, expandDis=1.0, goalSampleRate=5, maxIter=500): u""" Setting Parameter @@ -29,17 +31,17 @@ class RRT(): randArea:Ramdom Samping Area [min,max] """ - self.start=Node(start[0],start[1]) - self.end=Node(goal[0],goal[1]) + 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): + def Planning(self, animation=True): u""" - Pathplanning + Pathplanning animation: flag for animation on or off """ @@ -48,7 +50,8 @@ class RRT(): while True: # Random Sampling if random.randint(0, 100) > self.goalSampleRate: - rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(self.minrand, self.maxrand)] + rnd = [random.uniform(self.minrand, self.maxrand), random.uniform( + self.minrand, self.maxrand)] else: rnd = [self.end.x, self.end.y] @@ -57,7 +60,7 @@ class RRT(): # print(nind) # expand tree - nearestNode =self.nodeList[nind] + nearestNode = self.nodeList[nind] theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) newNode = copy.deepcopy(nearestNode) @@ -81,18 +84,17 @@ class RRT(): if animation: self.DrawGraph(rnd) - - path=[[self.end.x,self.end.y]] + 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]) + path.append([node.x, node.y]) lastIndex = node.parent path.append([self.start.x, self.start.y]) return path - def DrawGraph(self,rnd=None): + def DrawGraph(self, rnd=None): u""" Draw Graph """ @@ -102,8 +104,12 @@ class RRT(): 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") - plt.plot([ox for (ox,oy,size) in obstacleList],[oy for (ox,oy,size) in obstacleList], "ok", ms=size * 20) + plt.plot([node.x, self.nodeList[node.parent].x], [ + node.y, self.nodeList[node.parent].y], "-g") + + for (ox, oy, size) in 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]) @@ -111,7 +117,8 @@ class RRT(): plt.pause(0.01) def GetNearestListIndex(self, nodeList, rnd): - dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodeList] + dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) + ** 2 for node in nodeList] minind = dlist.index(min(dlist)) return minind @@ -126,6 +133,7 @@ class RRT(): return True # safe + class Node(): u""" RRT Node @@ -136,9 +144,10 @@ class Node(): self.y = y self.parent = None + if __name__ == '__main__': import matplotlib.pyplot as plt - #====Search Path with RRT==== + # ====Search Path with RRT==== obstacleList = [ (5, 5, 1), (3, 6, 2), @@ -147,13 +156,14 @@ if __name__ == '__main__': (7, 5, 2), (9, 5, 2) ] # [x,y,size] - #Set Initial parameters - rrt=RRT(start=[0,0],goal=[5,10],randArea=[-2,15],obstacleList=obstacleList) - path=rrt.Planning(animation=True) + # Set Initial parameters + 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') + 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() diff --git a/scripts/optimization/ConjugateGradientMethod/ConjugateGradientMethod.py b/scripts/optimization/ConjugateGradientMethod/ConjugateGradientMethod.py deleted file mode 100644 index dd6cfcbd..00000000 --- a/scripts/optimization/ConjugateGradientMethod/ConjugateGradientMethod.py +++ /dev/null @@ -1,96 +0,0 @@ -#!/usr/bin/python -# -*- coding: utf-8 -*- - -import matplotlib.pyplot as plt -import numpy as np -import random -import math - -delta = 0.1 -minXY=-5.0 -maxXY=5.0 -nContour=50 -alpha=0.001 - -def Jacob(state): - u""" - jacobi matrix of Himmelblau's function - """ - x=state[0] - y=state[1] - dx=4*x**3+4*x*y-44*x+2*x+2*y**2-14 - dy=2*x**2+4*x*y+4*y**3-26*y-22 - J=np.array([dx,dy]) - return J - -def HimmelblauFunction(x,y): - u""" - Himmelblau's function - see Himmelblau's function - Wikipedia, the free encyclopedia - http://en.wikipedia.org/wiki/Himmelblau%27s_function - """ - return (x**2+y-11)**2+(x+y**2-7)**2 - -def CreateMeshData(): - x = np.arange(minXY, maxXY, delta) - y = np.arange(minXY, maxXY, delta) - X, Y = np.meshgrid(x, y) - Z=[HimmelblauFunction(x,y) for (x,y) in zip(X,Y)] - return(X,Y,Z) - -def ConjugateGradientMethod(start,Jacob): - u""" - Conjugate Gradient Method Optimization - """ - - result=start - x=start - preJ=None - - while 1: - J=Jacob(x) - - #convergence check - sumJ=sum([abs(alpha*j) for j in J]) - if sumJ<=0.01: - print("OK") - break - - if preJ is not None: - beta=np.linalg.norm(J)**2/np.linalg.norm(preJ)**2 - grad=-1.0*J+beta*grad - - else: - grad=-1.0*J - - x=x+[alpha*g for g in grad] - result=np.vstack((result,x)) - # print(x) - - if math.isnan(x[0]): - print("nan") - break - - - preJ=-1.0*J - - - return result - -# Main -start=np.array([random.uniform(minXY,maxXY),random.uniform(minXY,maxXY)]) - -result=ConjugateGradientMethod(start,Jacob) -(X,Y,Z)=CreateMeshData() -CS = plt.contour(X, Y, Z,nContour) -# plt.clabel(CS, inline=1, fontsize=10) -# plt.title('Simplest default with labels') - -plt.plot(start[0],start[1],"xr"); - -optX=[x[0] for x in result] -optY=[x[1] for x in result] -plt.plot(optX,optY,"-r"); - -plt.show() - diff --git a/scripts/optimization/ConjugateGradientMethod/figure/figure_1.png b/scripts/optimization/ConjugateGradientMethod/figure/figure_1.png deleted file mode 100644 index db2b8d9f..00000000 Binary files a/scripts/optimization/ConjugateGradientMethod/figure/figure_1.png and /dev/null differ diff --git a/scripts/optimization/LagrangeMultiplierMethod/LagrangeMultiplierMethod.py b/scripts/optimization/LagrangeMultiplierMethod/LagrangeMultiplierMethod.py deleted file mode 100644 index 9b3c1280..00000000 --- a/scripts/optimization/LagrangeMultiplierMethod/LagrangeMultiplierMethod.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/python -# -*- coding: utf-8 -*- -import matplotlib.pyplot as plt -import numpy as np -import random -from math import * - -delta = 0.1 -minXY = -5.0 -maxXY = 5.0 -nContour = 50 - - -def dfunc(d): - x = d[0] - y = d[1] - l = d[2] - dx = -2 * l + 4 * x * (x ** 2 + y - 11) - dy = l + 2 * x * x + 2 * y - 22 - dl = -2 * x + y - 1 - return [dx, dy, dl] - - -def SampleFunc(x, y): - return (x ** 2 + y - 11) ** 2 - - -def ConstrainFunction(x): - return (2.0 * x + 1.0) - - -def CreateMeshData(): - x = np.arange(minXY, maxXY, delta) - y = np.arange(minXY, maxXY, delta) - X, Y = np.meshgrid(x, y) - Z = [SampleFunc(ix, iy) for (ix, iy) in zip(X, Y)] - return(X, Y, Z) - - -# Main -start = np.matrix([random.uniform(minXY, maxXY), - random.uniform(minXY, maxXY), 0]) - -(X, Y, Z) = CreateMeshData() -CS = plt.contour(X, Y, Z, nContour) - -Xc = np.arange(minXY, maxXY, delta) -Yc = [ConstrainFunction(x) for x in Xc] - -# plt.plot(start[0,0],start[0,1],"xr"); -plt.plot(Xc, Yc, "-r") - -# X1 = fsolve(dfunc, [-3, -3, 10]) -# print(X1) -# print(dfunc(X1)) - -# the answer from sympy -result = np.matrix([ - [-1, -1], - [-1 + sqrt(11), -1 + 2 * sqrt(11)], - [-sqrt(11) - 1, -2 * sqrt(11) - 1]]) -print(result) - -plt.plot(result[:, 0], result[:, 1], "or") - -plt.axis([minXY, maxXY, minXY, maxXY]) -plt.show() diff --git a/scripts/optimization/LagrangeMultiplierMethod/figure/figure_1.png b/scripts/optimization/LagrangeMultiplierMethod/figure/figure_1.png deleted file mode 100644 index 1adcd226..00000000 Binary files a/scripts/optimization/LagrangeMultiplierMethod/figure/figure_1.png and /dev/null differ diff --git a/scripts/optimization/NewtonMethod/NewtonMethod.py b/scripts/optimization/NewtonMethod/NewtonMethod.py deleted file mode 100644 index 1c809964..00000000 --- a/scripts/optimization/NewtonMethod/NewtonMethod.py +++ /dev/null @@ -1,94 +0,0 @@ -#!/usr/bin/python -# -*- coding: utf-8 -*- - -import matplotlib.pyplot as plt -import numpy as np -import random - -delta = 0.1 -minXY=-5.0 -maxXY=5.0 -nContour=50 -alpha=0.01 - -def Hessian(state): - u""" - Hessian matrix of Himmelblau's function - """ - x=state[0] - y=state[1] - dxx=12*x**2+4*y-42; - dxy=4*x+4*y - dyy=4*x+12*y**2-26 - H=np.array([[dxx,dxy],[dxy,dyy]]) - return H - - -def Jacob(state): - u""" - jacobi matrix of Himmelblau's function - """ - x=state[0] - y=state[1] - dx=4*x**3+4*x*y-44*x+2*x+2*y**2-14 - dy=2*x**2+4*x*y+4*y**3-26*y-22 - J=[dx,dy] - return J - -def HimmelblauFunction(x,y): - u""" - Himmelblau's function - see Himmelblau's function - Wikipedia, the free encyclopedia - http://en.wikipedia.org/wiki/Himmelblau%27s_function - """ - return (x**2+y-11)**2+(x+y**2-7)**2 - -def CreateMeshData(): - x = np.arange(minXY, maxXY, delta) - y = np.arange(minXY, maxXY, delta) - X, Y = np.meshgrid(x, y) - Z=[HimmelblauFunction(x,y) for (x,y) in zip(X,Y)] - return(X,Y,Z) - -def NewtonMethod(start,Jacob): - u""" - Newton Method Optimization - """ - - result=start - x=start - - while 1: - J=Jacob(x) - H=Hessian(x) - sumJ=sum([abs(alpha*j) for j in J]) - if sumJ<=0.01: - print("OK") - break - - grad=-np.linalg.inv(H).dot(J) - print(grad) - - x=x+[alpha*j for j in grad] - - result=np.vstack((result,x)) - - return result - -# Main -start=np.array([random.uniform(minXY,maxXY),random.uniform(minXY,maxXY)]) - -result=NewtonMethod(start,Jacob) -(X,Y,Z)=CreateMeshData() -CS = plt.contour(X, Y, Z,nContour) -# plt.clabel(CS, inline=1, fontsize=10) -# plt.title('Simplest default with labels') - -plt.plot(start[0],start[1],"xr"); - -optX=[x[0] for x in result] -optY=[x[1] for x in result] -plt.plot(optX,optY,"-r"); - -plt.show() - diff --git a/scripts/optimization/NewtonMethod/figure/figure_1.png b/scripts/optimization/NewtonMethod/figure/figure_1.png deleted file mode 100644 index 87befb59..00000000 Binary files a/scripts/optimization/NewtonMethod/figure/figure_1.png and /dev/null differ diff --git a/scripts/optimization/QuasiNewtonMethod/QuasiNewtonMethod.py b/scripts/optimization/QuasiNewtonMethod/QuasiNewtonMethod.py deleted file mode 100644 index 407fe19e..00000000 --- a/scripts/optimization/QuasiNewtonMethod/QuasiNewtonMethod.py +++ /dev/null @@ -1,89 +0,0 @@ -#!/usr/bin/python -# -*- coding: utf-8 -*- - -import matplotlib.pyplot as plt -import numpy as np -import random -import math - -delta = 0.1 -minXY=-5.0 -maxXY=5.0 -nContour=50 -alpha=0.001 - -def Jacob(state): - u""" - jacobi matrix of Himmelblau's function - """ - x=state[0,0] - y=state[0,1] - dx=4*x**3+4*x*y-44*x+2*x+2*y**2-14 - dy=2*x**2+4*x*y+4*y**3-26*y-22 - J=np.matrix([dx,dy]).T - return J - -def HimmelblauFunction(x,y): - u""" - Himmelblau's function - see Himmelblau's function - Wikipedia, the free encyclopedia - http://en.wikipedia.org/wiki/Himmelblau%27s_function - """ - return (x**2+y-11)**2+(x+y**2-7)**2 - -def CreateMeshData(): - x = np.arange(minXY, maxXY, delta) - y = np.arange(minXY, maxXY, delta) - X, Y = np.meshgrid(x, y) - Z=[HimmelblauFunction(x,y) for (x,y) in zip(X,Y)] - return(X,Y,Z) - -def QuasiNewtonMethod(start,Jacob): - u""" - Quasi Newton Method Optimization - """ - - result=start - x=start - - H= np.identity(2) - preJ=None - preG=None - - while 1: - J=Jacob(x) - - sumJ=abs(np.sum(J)) - if sumJ<=0.01: - print("OK") - break - - grad=-np.linalg.inv(H)*J - x+=alpha*grad.T - - result=np.vstack((result,np.array(x))) - - if preJ is not None: - y=J-preJ - H=H+(y*y.T)/(y.T*preG)-(H*preG*preG.T*H)/(preG.T*H*preG) - - preJ=J - preG=(alpha*grad.T).T - - return result - -# Main -start=np.matrix([random.uniform(minXY,maxXY),random.uniform(minXY,maxXY)]) - -result=QuasiNewtonMethod(start,Jacob) -(X,Y,Z)=CreateMeshData() -CS = plt.contour(X, Y, Z,nContour) - -plt.plot(start[0,0],start[0,1],"xr"); - -optX=result[:,0] -optY=result[:,1] -plt.plot(optX,optY,"-r"); - -plt.show() - diff --git a/scripts/optimization/QuasiNewtonMethod/figure/figure_1.png b/scripts/optimization/QuasiNewtonMethod/figure/figure_1.png deleted file mode 100644 index 040843fc..00000000 Binary files a/scripts/optimization/QuasiNewtonMethod/figure/figure_1.png and /dev/null differ diff --git a/scripts/optimization/SteepestDescentMethod/SteepestDescentMethod.py b/scripts/optimization/SteepestDescentMethod/SteepestDescentMethod.py deleted file mode 100644 index ad394636..00000000 --- a/scripts/optimization/SteepestDescentMethod/SteepestDescentMethod.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/usr/bin/python -# -*- coding: utf-8 -*- -import matplotlib.pyplot as plt -import numpy as np -import random - -delta = 0.1 -minXY = -5.0 -maxXY = 5.0 -nContour = 50 -alpha = 0.01 - - -def Jacob(state): - u""" - jacobi matrix of Himmelblau's function - """ - x = state[0, 0] - y = state[0, 1] - dx = 4 * x ** 3 + 4 * x * y - 44 * x + 2 * x + 2 * y ** 2 - 14 - dy = 2 * x ** 2 + 4 * x * y + 4 * y ** 3 - 26 * y - 22 - J = np.matrix([dx, dy]) - return J - - -def HimmelblauFunction(x, y): - u""" - Himmelblau's function - see Himmelblau's function - Wikipedia, the free encyclopedia - http://en.wikipedia.org/wiki/Himmelblau%27s_function - """ - return (x ** 2 + y - 11) ** 2 + (x + y ** 2 - 7) ** 2 - - -def ConstrainFunction(x): - return (2.0 * x + 1.0) - - -def CreateMeshData(): - x = np.arange(minXY, maxXY, delta) - y = np.arange(minXY, maxXY, delta) - X, Y = np.meshgrid(x, y) - Z = [HimmelblauFunction(ix, iy) for (ix, iy) in zip(X, Y)] - return(X, Y, Z) - - -def SteepestDescentMethod(start, Jacob): - u""" - Steepest Descent Method Optimization - """ - - result = start - x = start - - while 1: - J = Jacob(x) - sumJ = np.sum(abs(alpha * J)) - if sumJ <= 0.01: - print("OK") - break - - x = x - alpha * J - result = np.vstack((result, x)) - - return result - - -# Main -start = np.matrix([random.uniform(minXY, maxXY), random.uniform(minXY, maxXY)]) - -result = SteepestDescentMethod(start, Jacob) -(X, Y, Z) = CreateMeshData() -CS = plt.contour(X, Y, Z, nContour) - -Xc = np.arange(minXY, maxXY, delta) -Yc = [ConstrainFunction(x) for x in Xc] - -plt.plot(start[0, 0], start[0, 1], "xr") - -plt.plot(result[:, 0], result[:, 1], "-r") - -plt.axis([minXY, maxXY, minXY, maxXY]) -plt.show() diff --git a/scripts/optimization/SteepestDescentMethod/figure/figure_1.png b/scripts/optimization/SteepestDescentMethod/figure/figure_1.png deleted file mode 100644 index e6be9bc0..00000000 Binary files a/scripts/optimization/SteepestDescentMethod/figure/figure_1.png and /dev/null differ