From b59987e7edd30d88a54f16f2c9f6ffa810bd8f58 Mon Sep 17 00:00:00 2001
From: AtsushiSakai <asakai.amsl+github@gmail.com>
Date: Mon, 10 Jul 2017 22:52:21 -0700
Subject: [PATCH] add trajectory optimizer

---
 .../model_predictive_trajectory_generator.py  | 103 +++++++++++++++++-
 1 file changed, 100 insertions(+), 3 deletions(-)

diff --git a/PathPlanning/LatticePlanner/model_predictive_trajectory_generator.py b/PathPlanning/LatticePlanner/model_predictive_trajectory_generator.py
index b5757219..8dd5dc35 100644
--- a/PathPlanning/LatticePlanner/model_predictive_trajectory_generator.py
+++ b/PathPlanning/LatticePlanner/model_predictive_trajectory_generator.py
@@ -38,14 +38,15 @@ def generate_trajectory(s, km, kf, k0):
     #  plt.show()
 
     state = State()
-    x, y = [state.x], [state.y]
+    x, y, yaw = [state.x], [state.y], [state.yaw]
 
     for ikp in kp:
         state = update(state, v, ikp, dt, L)
         x.append(state.x)
         y.append(state.y)
+        yaw.append(state.yaw)
 
-    return x, y
+    return x, y, yaw
 
 
 def update(state, v, delta, dt, L):
@@ -69,6 +70,101 @@ def pi_2_pi(angle):
     return angle
 
 
+def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
+    u"""
+    Plot arrow
+    """
+    plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
+              fc=fc, ec=ec, head_width=width, head_length=width)
+    plt.plot(x, y)
+    plt.plot(0, 0)
+
+
+def calc_diff(target, x, y, yaw):
+    d = np.array([x[-1] - target.x, y[-1] - target.y, yaw[-1] - target.yaw])
+    return d
+
+
+def calc_J(target, p, h, k0):
+    xp, yp, yawp = generate_trajectory(p[0, 0] + h[0, 0], p[1, 0], p[2, 0], k0)
+    dp = calc_diff(target, xp, yp, yawp)
+    d1 = np.matrix(dp / h[0, 0]).T
+
+    xp, yp, yawp = generate_trajectory(p[0, 0], p[1, 0] + h[1, 0], p[2, 0], k0)
+    dp = calc_diff(target, xp, yp, yawp)
+    #  xn, yn, yawn = generate_trajectory(p[0, 0], p[1, 0] - h[1, 0], p[2, 0], k0)
+    #  dn = calc_diff(target, xn, yn, yawn)
+    #  d2 = np.matrix((dp - dn) / 2.0 * h[1, 0]).T
+    d2 = np.matrix(dp / h[1, 0]).T
+
+    xp, yp, yawp = generate_trajectory(p[0, 0], p[1, 0], p[2, 0] + h[2, 0], k0)
+    dp = calc_diff(target, xp, yp, yawp)
+    #  xn, yn, yawn = generate_trajectory(p[0, 0], p[1, 0], p[2, 0] - h[2, 0], k0)
+    #  dn = calc_diff(target, xn, yn, yawn)
+    #  d3 = np.matrix((dp - dn) / 2.0 * h[2, 0]).T
+    d3 = np.matrix(dp / h[2, 0]).T
+    #  print(d1, d2, d3)
+
+    J = np.hstack((d1, d2, d3))
+    #  print(J)
+
+    return J
+
+
+def optimize_trajectory(target, k0):
+
+    p = np.matrix([5.0, 0.0, 0.0]).T
+    h = np.matrix([0.1, 0.003, 0.003]).T
+
+    for i in range(1000):
+        xc, yc, yawc = generate_trajectory(p[0], p[1], p[2], k0)
+        dc = np.matrix(calc_diff(target, xc, yc, yawc)).T
+
+        if np.linalg.norm(dc) <= 0.1:
+            break
+
+        J = calc_J(target, p, h, k0)
+
+        dp = - np.linalg.inv(J) * dc
+
+        p += np.array(dp)
+
+        #  print(p)
+        #  plt.clf()
+        #  plot_arrow(target.x, target.y, target.yaw)
+        #  plt.plot(xc, yc, "-r")
+        #  plt.axis("equal")
+        #  plt.grid(True)
+        #  #  plt.show()
+        #  plt.pause(0.1)
+
+    plot_arrow(target.x, target.y, target.yaw)
+    plt.plot(xc, yc, "-r")
+    plt.axis("equal")
+    plt.grid(True)
+
+    print("done")
+
+
+def test_optimize_trajectory():
+
+    target = State(x=5.0, y=2.0, yaw=math.radians(00.0))
+    k0 = 0.0
+    #  s = 5.0  # [m]
+    #  km = math.radians(30.0)
+    #  kf = math.radians(-30.0)
+
+    optimize_trajectory(target, k0)
+
+    #  x, y = generate_trajectory(s, km, kf, k0)
+
+    #  plt.plot(x, y, "-r")
+    plot_arrow(target.x, target.y, target.yaw)
+    plt.axis("equal")
+    plt.grid(True)
+    plt.show()
+
+
 def test_trajectory_generate():
     s = 5.0  # [m]
     k0 = 0.0
@@ -89,7 +185,8 @@ def test_trajectory_generate():
 
 def main():
     print(__file__ + " start!!")
-    test_trajectory_generate()
+    #  test_trajectory_generate()
+    test_optimize_trajectory()
 
 
 if __name__ == '__main__':