initail commit for dubins_path_planning code

scripts/PathPlanning/DubinsPath/dubins_path_planning.py

@@ -0,0 +1,15 @@
+#! /usr/bin/python
+# -*- coding: utf-8 -*-
+"""
+
+Dubins path planner sample code
+
+author Atsushi Sakai(@Atsushi_twi)
+
+Liscense MIT
+
+"""
+
+if __name__ == '__main__':
+    print("Dubins path planner sample start!!")
+    pass

scripts/optimization/LagrangeMultiplierMethod/LagrangeMultiplierMethod.py

@@ -4,12 +4,22 @@ 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
 
+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
 
@@ -35,11 +45,15 @@ 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]])
+#  [-1+sqrt(11),-1+2*sqrt(11)],
+#  [-sqrt(11)-1,-2*sqrt(11)-1]])
 print(result)
 
 plt.plot(result[:,0],result[:,1],"or");