From 62508ba9360795c4039057dd649f2ebe2ced28a4 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Fri, 13 Jul 2018 18:59:32 +0900
Subject: [PATCH] some animation is added

---
 .../Eta3SplinePath/eta3_spline_path.py        | 72 +++++++++++++++++--
 doc/PythonRoboticsPaper                       |  2 +-
 2 files changed, 69 insertions(+), 5 deletions(-)

diff --git a/PathPlanning/Eta3SplinePath/eta3_spline_path.py b/PathPlanning/Eta3SplinePath/eta3_spline_path.py
index 54ea33c7..2fcac09f 100644
--- a/PathPlanning/Eta3SplinePath/eta3_spline_path.py
+++ b/PathPlanning/Eta3SplinePath/eta3_spline_path.py
@@ -3,6 +3,7 @@
 \eta^3 polynomials planner
 
 author: Joe Dinius, Ph.D (https://jwdinius.github.io)
+        Atsushi Sakai (@Atsushi_twi)
 
 Ref:
 
@@ -162,10 +163,63 @@ class eta3_path_segment(object):
         return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7]))
 
 
-def main():
-    """
-    recreate path from reference (see Table 1)
-    """
+def test1():
+
+    for i in range(10):
+        path_segments = []
+        # segment 1: lane-change curve
+        start_pose = [0, 0, 0]
+        end_pose = [4, 3.0, 0]
+        # NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative
+        kappa = [0, 0, 0, 0]
+        eta = [i, i, 0, 0, 0, 0]
+        path_segments.append(eta3_path_segment(
+            start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
+
+        path = eta3_path(path_segments)
+
+        # interpolate at several points along the path
+        ui = np.linspace(0, len(path_segments), 1001)
+        pos = np.empty((2, ui.size))
+        for i, u in enumerate(ui):
+            pos[:, i] = path.calc_path_point(u)
+
+        # plot the path
+        plt.plot(pos[0, :], pos[1, :])
+        plt.pause(1.0)
+
+    plt.close("all")
+
+
+def test2():
+
+    for i in range(10):
+        path_segments = []
+        # segment 1: lane-change curve
+        start_pose = [0, 0, 0]
+        end_pose = [4, 3.0, 0]
+        # NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative
+        kappa = [0, 0, 0, 0]
+        eta = [0, 0, (i - 5) * 20, (5 - i) * 20, 0, 0]
+        path_segments.append(eta3_path_segment(
+            start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
+
+        path = eta3_path(path_segments)
+
+        # interpolate at several points along the path
+        ui = np.linspace(0, len(path_segments), 1001)
+        pos = np.empty((2, ui.size))
+        for i, u in enumerate(ui):
+            pos[:, i] = path.calc_path_point(u)
+
+        # plot the path
+        plt.plot(pos[0, :], pos[1, :])
+        plt.pause(1.0)
+
+    plt.close("all")
+
+
+def test3():
     path_segments = []
 
     # segment 1: lane-change curve
@@ -224,8 +278,18 @@ def main():
     plt.xlabel('x')
     plt.ylabel('y')
     plt.title('Path')
+
     plt.show()
 
 
+def main():
+    """
+    recreate path from reference (see Table 1)
+    """
+    test1()
+    test2()
+    test3()
+
+
 if __name__ == '__main__':
     main()
diff --git a/doc/PythonRoboticsPaper b/doc/PythonRoboticsPaper
index 9123d0a4..53282fa7 160000
--- a/doc/PythonRoboticsPaper
+++ b/doc/PythonRoboticsPaper
@@ -1 +1 @@
-Subproject commit 9123d0a4929488116ff2f9f76cd8629a5dc5fba3
+Subproject commit 53282fa7fccaa0ac02f94c5de2e08bae5bf4a356