From ac4abdc493eaac34b00de07896199da72d4d9f61 Mon Sep 17 00:00:00 2001
From: AtsushiSakai <asakai.amsl+github@gmail.com>
Date: Sat, 29 Apr 2017 09:17:15 -0700
Subject: [PATCH] initail commit for dubins_path_planning code

---
 .../DubinsPath/dubins_path_planning.py         | 15 +++++++++++++++
 .../LagrangeMultiplierMethod.py                | 18 ++++++++++++++++--
 2 files changed, 31 insertions(+), 2 deletions(-)
 create mode 100644 scripts/PathPlanning/DubinsPath/dubins_path_planning.py

diff --git a/scripts/PathPlanning/DubinsPath/dubins_path_planning.py b/scripts/PathPlanning/DubinsPath/dubins_path_planning.py
new file mode 100644
index 00000000..f7e2b2c8
--- /dev/null
+++ b/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
diff --git a/scripts/optimization/LagrangeMultiplierMethod/LagrangeMultiplierMethod.py b/scripts/optimization/LagrangeMultiplierMethod/LagrangeMultiplierMethod.py
index 5c64e4ba..12a1fb8d 100644
--- a/scripts/optimization/LagrangeMultiplierMethod/LagrangeMultiplierMethod.py
+++ b/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");