mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-14 12:37:57 -05:00
add code
This commit is contained in:
BIN
scripts/optimization/ConjugateGradientMethod/figure/figure_1.png
Normal file
BIN
scripts/optimization/ConjugateGradientMethod/figure/figure_1.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 219 KiB |
@@ -4,60 +4,64 @@ import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import random
|
||||
from math import *
|
||||
from scipy.optimize import fsolve
|
||||
|
||||
delta = 0.1
|
||||
minXY=-5.0
|
||||
maxXY=5.0
|
||||
nContour=50
|
||||
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]
|
||||
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 SampleFunc(x,y):
|
||||
return (x**2+y-11)**2
|
||||
|
||||
def ConstrainFunction(x):
|
||||
return (2.0*x+1.0)
|
||||
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(x,y) for (x,y) in zip(X,Y)]
|
||||
return(X,Y,Z)
|
||||
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])
|
||||
start = np.matrix([random.uniform(minXY, maxXY),
|
||||
random.uniform(minXY, maxXY), 0])
|
||||
|
||||
(X,Y,Z)=CreateMeshData()
|
||||
CS = plt.contour(X, Y, Z,nContour)
|
||||
(X, Y, Z) = CreateMeshData()
|
||||
CS = plt.contour(X, Y, Z, nContour)
|
||||
|
||||
Xc=np.arange(minXY,maxXY,delta)
|
||||
Yc=[ConstrainFunction(x) for x in Xc]
|
||||
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");
|
||||
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]])
|
||||
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.plot(result[:, 0], result[:, 1], "or")
|
||||
|
||||
plt.axis([minXY, maxXY, minXY, maxXY])
|
||||
plt.show()
|
||||
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 247 KiB |
BIN
scripts/optimization/NewtonMethod/figure/figure_1.png
Normal file
BIN
scripts/optimization/NewtonMethod/figure/figure_1.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 226 KiB |
BIN
scripts/optimization/QuasiNewtonMethod/figure/figure_1.png
Normal file
BIN
scripts/optimization/QuasiNewtonMethod/figure/figure_1.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 226 KiB |
@@ -3,77 +3,81 @@
|
||||
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.01
|
||||
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])
|
||||
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):
|
||||
|
||||
def HimmelblauFunction(x, y):
|
||||
u"""
|
||||
Himmelblau's function
|
||||
see Himmelblau's function - Wikipedia, the free encyclopedia
|
||||
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
|
||||
return (x ** 2 + y - 11) ** 2 + (x + y ** 2 - 7) ** 2
|
||||
|
||||
|
||||
def ConstrainFunction(x):
|
||||
return (2.0*x+1.0)
|
||||
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(x,y) for (x,y) in zip(X,Y)]
|
||||
return(X,Y,Z)
|
||||
Z = [HimmelblauFunction(ix, iy) for (ix, iy) in zip(X, Y)]
|
||||
return(X, Y, Z)
|
||||
|
||||
def SteepestDescentMethod(start,Jacob):
|
||||
|
||||
def SteepestDescentMethod(start, Jacob):
|
||||
u"""
|
||||
Steepest Descent Method Optimization
|
||||
"""
|
||||
|
||||
result=start
|
||||
x=start
|
||||
result = start
|
||||
x = start
|
||||
|
||||
while 1:
|
||||
J=Jacob(x)
|
||||
sumJ=np.sum(abs(alpha*J))
|
||||
if sumJ<=0.01:
|
||||
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))
|
||||
x = x - alpha * J
|
||||
result = np.vstack((result, x))
|
||||
|
||||
return result
|
||||
|
||||
|
||||
# Main
|
||||
start=np.matrix([random.uniform(minXY,maxXY),random.uniform(minXY,maxXY)])
|
||||
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)
|
||||
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]
|
||||
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");
|
||||
plt.plot(start[0, 0], start[0, 1], "xr")
|
||||
|
||||
plt.plot(result[:,0],result[:,1],"-r");
|
||||
plt.plot(result[:, 0], result[:, 1], "-r")
|
||||
|
||||
plt.axis([minXY, maxXY, minXY, maxXY])
|
||||
plt.show()
|
||||
|
||||
BIN
scripts/optimization/SteepestDescentMethod/figure/figure_1.png
Normal file
BIN
scripts/optimization/SteepestDescentMethod/figure/figure_1.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 226 KiB |
Reference in New Issue
Block a user