remove optimization sample

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
 

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()

scripts/optimization/ConjugateGradientMethod/ConjugateGradientMethod.py

@@ -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()
-

scripts/optimization/LagrangeMultiplierMethod/LagrangeMultiplierMethod.py

@@ -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()

scripts/optimization/NewtonMethod/NewtonMethod.py

@@ -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()
-

scripts/optimization/QuasiNewtonMethod/QuasiNewtonMethod.py

@@ -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()
-

scripts/optimization/SteepestDescentMethod/SteepestDescentMethod.py

@@ -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()