From c2debe3b05b095046f63f72bb9b9c3b0c52c2635 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Thu, 31 Dec 2020 22:54:40 +0900
Subject: [PATCH] use pytest for test runner (#452)

---
 .github/pull_request_template.md              |   2 +-
 .../two_joint_arm_to_point_control.py         |  26 ++-
 .../Eta3SplinePath/eta3_spline_path.py        | 176 ++++++++++--------
 .../eta3_spline_trajectory.py                 |  48 ++---
 README.md                                     |   2 +
 requirements.txt                              |   1 +
 runtests.sh                                   |   8 +-
 tests/__init__.py                             |   0
 8 files changed, 151 insertions(+), 112 deletions(-)
 create mode 100644 tests/__init__.py

diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md
index 9fbb893c..fb412d57 100644
--- a/.github/pull_request_template.md
+++ b/.github/pull_request_template.md
@@ -1,7 +1,7 @@
 <!-- 
 Thanks for contributing a pull request! 
 Please ensure that your PR satisfies the checklist before submitting:
-- [] This project only accept codes for python 3.8 or higher.
+- [] This project only accept codes for python 3.9 or higher.
 - [] If you add a new algorithm sample code, please add a unit test file under `test` dir.
      This sample test code might help you : https://github.com/AtsushiSakai/PythonRobotics/blob/master/tests/test_a_star.py
 - [] If you fix a bug of existing code please add a test function in the test code to show the issue was solved.
diff --git a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
index 90fcea1d..9f835c8b 100644
--- a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
+++ b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
@@ -5,8 +5,10 @@ Left-click the plot to set the goal position of the end effector
 Author: Daniel Ingram (daniel-s-ingram)
         Atsushi Sakai (@Atsushi_twi)
 
-Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7 p102
-- [Robotics, Vision and Control \| SpringerLink](https://link.springer.com/book/10.1007/978-3-642-20144-8)
+Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017,
+ ISBN 978-3-319-54413-7 p102
+- [Robotics, Vision and Control]
+(https://link.springer.com/book/10.1007/978-3-642-20144-8)
 
 """
 
@@ -37,6 +39,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
     When out of bounds, rewrite x and y with last correct values
     """
     global x, y
+    x_prev, y_prev = None, None
     while True:
         try:
             if x is not None and y is not None:
@@ -47,18 +50,20 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
             else:
                 theta2_goal = np.arccos(
                     (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2))
-            theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2 *
-                                                              np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal)))
+            tmp = np.math.atan2(l2 * np.sin(theta2_goal),
+                                (l1 + l2 * np.cos(theta2_goal)))
+            theta1_goal = np.math.atan2(y, x) - tmp
 
             if theta1_goal < 0:
                 theta2_goal = -theta2_goal
-                theta1_goal = np.math.atan2(
-                    y, x) - np.math.atan2(l2 * np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal)))
+                tmp = np.math.atan2(l2 * np.sin(theta2_goal),
+                                    (l1 + l2 * np.cos(theta2_goal)))
+                theta1_goal = np.math.atan2(y, x) - tmp
 
             theta1 = theta1 + Kp * ang_diff(theta1_goal, theta1) * dt
             theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt
         except ValueError as e:
-            print("Unreachable goal")
+            print("Unreachable goal"+e)
         except TypeError:
             x = x_prev
             y = y_prev
@@ -66,6 +71,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
         wrist = plot_arm(theta1, theta2, x, y)
 
         # check goal
+        d2goal = None
         if x is not None and y is not None:
             d2goal = np.hypot(wrist[0] - x, wrist[1] - y)
 
@@ -73,7 +79,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
             return theta1, theta2
 
 
-def plot_arm(theta1, theta2, x, y):  # pragma: no cover
+def plot_arm(theta1, theta2, target_x, target_y):  # pragma: no cover
     shoulder = np.array([0, 0])
     elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])
     wrist = elbow + \
@@ -89,8 +95,8 @@ def plot_arm(theta1, theta2, x, y):  # pragma: no cover
         plt.plot(elbow[0], elbow[1], 'ro')
         plt.plot(wrist[0], wrist[1], 'ro')
 
-        plt.plot([wrist[0], x], [wrist[1], y], 'g--')
-        plt.plot(x, y, 'g*')
+        plt.plot([wrist[0], target_x], [wrist[1], target_y], 'g--')
+        plt.plot(target_x, target_y, 'g*')
 
         plt.xlim(-2, 2)
         plt.ylim(-2, 2)
diff --git a/PathPlanning/Eta3SplinePath/eta3_spline_path.py b/PathPlanning/Eta3SplinePath/eta3_spline_path.py
index 414f6b45..dc07d3c8 100644
--- a/PathPlanning/Eta3SplinePath/eta3_spline_path.py
+++ b/PathPlanning/Eta3SplinePath/eta3_spline_path.py
@@ -1,13 +1,13 @@
 """
 
-\eta^3 polynomials planner
+eta^3 polynomials planner
 
 author: Joe Dinius, Ph.D (https://jwdinius.github.io)
         Atsushi Sakai (@Atsushi_twi)
 
 Ref:
-
-- [\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots](https://ieeexplore.ieee.org/document/4339545/)
+- [eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots]
+(https://ieeexplore.ieee.org/document/4339545/)
 
 """
 
@@ -15,23 +15,25 @@ import numpy as np
 import matplotlib.pyplot as plt
 from scipy.integrate import quad
 
-# NOTE: *_pose is a 3-array: 0 - x coord, 1 - y coord, 2 - orientation angle \theta
+# NOTE: *_pose is a 3-array:
+# 0 - x coord, 1 - y coord, 2 - orientation angle \theta
 
 show_animation = True
 
 
-class eta3_path(object):
+class Eta3Path(object):
     """
-    eta3_path
+    Eta3Path
 
     input
-        segments: list of `eta3_path_segment` instances definining a continuous path
+        segments: a list of `Eta3PathSegment` instances
+        defining a continuous path
     """
 
     def __init__(self, segments):
         # ensure input has the correct form
         assert(isinstance(segments, list) and isinstance(
-            segments[0], eta3_path_segment))
+            segments[0], Eta3PathSegment))
         # ensure that each segment begins from the previous segment's end (continuity)
         for r, s in zip(segments[:-1], segments[1:]):
             assert(np.array_equal(r.end_pose, s.start_pose))
@@ -39,7 +41,7 @@ class eta3_path(object):
 
     def calc_path_point(self, u):
         """
-        eta3_path::calc_path_point
+        Eta3Path::calc_path_point
 
         input
             normalized interpolation point along path object, 0 <= u <= len(self.segments)
@@ -47,7 +49,7 @@ class eta3_path(object):
             2d (x,y) position vector
         """
 
-        assert(u >= 0 and u <= len(self.segments))
+        assert(0 <= u <= len(self.segments))
         if np.isclose(u, len(self.segments)):
             segment_idx = len(self.segments) - 1
             u = 1.
@@ -57,11 +59,12 @@ class eta3_path(object):
         return self.segments[segment_idx].calc_point(u)
 
 
-class eta3_path_segment(object):
+class Eta3PathSegment(object):
     """
-    eta3_path_segment - constructs an eta^3 path segment based on desired shaping, eta, and curvature vector, kappa.
-                        If either, or both, of eta and kappa are not set during initialization, they will
-                        default to zeros.
+    Eta3PathSegment - constructs an eta^3 path segment based on desired
+    shaping, eta, and curvature vector, kappa. If either, or both,
+    of eta and kappa are not set during initialization,
+    they will default to zeros.
 
     input
         start_pose - starting pose array  (x, y, \theta)
@@ -110,70 +113,96 @@ class eta3_path_segment(object):
         self.coeffs[1, 3] = 1. / 6 * eta[4] * sa + 1. / 6 * \
             (eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * ca
         # quartic (u^4)
-        self.coeffs[0, 4] = 35. * (end_pose[0] - start_pose[0]) - (20. * eta[0] + 5 * eta[2] + 2. / 3 * eta[4]) * ca \
-            + (5. * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa \
-            - (15. * eta[1] - 5. / 2 * eta[3] + 1. / 6 * eta[5]) * cb \
-            - (5. / 2 * eta[1]**2 * kappa[2] - 1. / 6 * eta[1] **
-               3 * kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb
-        self.coeffs[1, 4] = 35. * (end_pose[1] - start_pose[1]) - (20. * eta[0] + 5. * eta[2] + 2. / 3 * eta[4]) * sa \
-            - (5. * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca \
-            - (15. * eta[1] - 5. / 2 * eta[3] + 1. / 6 * eta[5]) * sb \
-            + (5. / 2 * eta[1]**2 * kappa[2] - 1. / 6 * eta[1] **
-               3 * kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
+        tmp1 = 35. * (end_pose[0] - start_pose[0])
+        tmp2 = (20. * eta[0] + 5 * eta[2] + 2. / 3 * eta[4]) * ca
+        tmp3 = (5. * eta[0] ** 2 * kappa[0] + 2. / 3 * eta[0] ** 3 * kappa[1]
+                + 2. * eta[0] * eta[2] * kappa[0]) * sa
+        tmp4 = (15. * eta[1] - 5. / 2 * eta[3] + 1. / 6 * eta[5]) * cb
+        tmp5 = (5. / 2 * eta[1] ** 2 * kappa[2] - 1. / 6 * eta[1] ** 3 *
+                kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb
+        self.coeffs[0, 4] = tmp1 - tmp2 + tmp3 - tmp4 - tmp5
+        tmp1 = 35. * (end_pose[1] - start_pose[1])
+        tmp2 = (20. * eta[0] + 5. * eta[2] + 2. / 3 * eta[4]) * sa
+        tmp3 = (5. * eta[0] ** 2 * kappa[0] + 2. / 3 * eta[0] ** 3 * kappa[1]
+                + 2. * eta[0] * eta[2] * kappa[0]) * ca
+        tmp4 = (15. * eta[1] - 5. / 2 * eta[3] + 1. / 6 * eta[5]) * sb
+        tmp5 = (5. / 2 * eta[1] ** 2 * kappa[2] - 1. / 6 * eta[1] ** 3 *
+                kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
+        self.coeffs[1, 4] = tmp1 - tmp2 - tmp3 - tmp4 + tmp5
         # quintic (u^5)
-        self.coeffs[0, 5] = -84. * (end_pose[0] - start_pose[0]) + (45. * eta[0] + 10. * eta[2] + eta[4]) * ca \
-            - (10. * eta[0]**2 * kappa[0] + eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * sa \
-            + (39. * eta[1] - 7. * eta[3] + 1. / 2 * eta[5]) * cb \
-            + (7. * eta[1]**2 * kappa[2] - 1. / 2 * eta[1]**3 *
-               kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb
-        self.coeffs[1, 5] = -84. * (end_pose[1] - start_pose[1]) + (45. * eta[0] + 10. * eta[2] + eta[4]) * sa \
-            + (10. * eta[0]**2 * kappa[0] + eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * ca \
-            + (39. * eta[1] - 7. * eta[3] + 1. / 2 * eta[5]) * sb \
-            - (7. * eta[1]**2 * kappa[2] - 1. / 2 * eta[1]**3 *
-               kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * cb
+        tmp1 = -84. * (end_pose[0] - start_pose[0])
+        tmp2 = (45. * eta[0] + 10. * eta[2] + eta[4]) * ca
+        tmp3 = (10. * eta[0] ** 2 * kappa[0] + eta[0] ** 3 * kappa[1] + 3. *
+                eta[0] * eta[2] * kappa[0]) * sa
+        tmp4 = (39. * eta[1] - 7. * eta[3] + 1. / 2 * eta[5]) * cb
+        tmp5 = + (7. * eta[1] ** 2 * kappa[2] - 1. / 2 * eta[1] ** 3 * kappa[3]
+                  - 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb
+        self.coeffs[0, 5] = tmp1 + tmp2 - tmp3 + tmp4 + tmp5
+        tmp1 = -84. * (end_pose[1] - start_pose[1])
+        tmp2 = (45. * eta[0] + 10. * eta[2] + eta[4]) * sa
+        tmp3 = (10. * eta[0] ** 2 * kappa[0] + eta[0] ** 3 * kappa[1] + 3. *
+                eta[0] * eta[2] * kappa[0]) * ca
+        tmp4 = (39. * eta[1] - 7. * eta[3] + 1. / 2 * eta[5]) * sb
+        tmp5 = - (7. * eta[1] ** 2 * kappa[2] - 1. / 2 * eta[1] ** 3 * kappa[3]
+                  - 3. / 2 * eta[1] * eta[3] * kappa[2]) * cb
+        self.coeffs[1, 5] = tmp1 + tmp2 + tmp3 + tmp4 + tmp5
         # sextic (u^6)
-        self.coeffs[0, 6] = 70. * (end_pose[0] - start_pose[0]) - (36. * eta[0] + 15. / 2 * eta[2] + 2. / 3 * eta[4]) * ca \
-            + (15. / 2 * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa \
-            - (34. * eta[1] - 13. / 2 * eta[3] + 1. / 2 * eta[5]) * cb \
-            - (13. / 2 * eta[1]**2 * kappa[2] - 1. / 2 * eta[1] **
-               3 * kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb
-        self.coeffs[1, 6] = 70. * (end_pose[1] - start_pose[1]) - (36. * eta[0] + 15. / 2 * eta[2] + 2. / 3 * eta[4]) * sa \
-            - (15. / 2 * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca \
-            - (34. * eta[1] - 13. / 2 * eta[3] + 1. / 2 * eta[5]) * sb \
-            + (13. / 2 * eta[1]**2 * kappa[2] - 1. / 2 * eta[1] **
-               3 * kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * cb
+        tmp1 = 70. * (end_pose[0] - start_pose[0])
+        tmp2 = (36. * eta[0] + 15. / 2 * eta[2] + 2. / 3 * eta[4]) * ca
+        tmp3 = + (15. / 2 * eta[0] ** 2 * kappa[0] + 2. / 3 * eta[0] ** 3 *
+                  kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa
+        tmp4 = (34. * eta[1] - 13. / 2 * eta[3] + 1. / 2 * eta[5]) * cb
+        tmp5 = - (13. / 2 * eta[1] ** 2 * kappa[2] - 1. / 2 * eta[1] ** 3 *
+                  kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb
+        self.coeffs[0, 6] = tmp1 - tmp2 + tmp3 - tmp4 + tmp5
+        tmp1 = 70. * (end_pose[1] - start_pose[1])
+        tmp2 = - (36. * eta[0] + 15. / 2 * eta[2] + 2. / 3 * eta[4]) * sa
+        tmp3 = - (15. / 2 * eta[0] ** 2 * kappa[0] + 2. / 3 * eta[0] ** 3 *
+                  kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca
+        tmp4 = - (34. * eta[1] - 13. / 2 * eta[3] + 1. / 2 * eta[5]) * sb
+        tmp5 = + (13. / 2 * eta[1] ** 2 * kappa[2] - 1. / 2 * eta[1] ** 3 *
+                  kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * cb
+        self.coeffs[1, 6] = tmp1 + tmp2 + tmp3 + tmp4 + tmp5
         # septic (u^7)
-        self.coeffs[0, 7] = -20. * (end_pose[0] - start_pose[0]) + (10. * eta[0] + 2. * eta[2] + 1. / 6 * eta[4]) * ca \
-            - (2. * eta[0]**2 * kappa[0] + 1. / 6 * eta[0]**3 * kappa[1] + 1. / 2 * eta[0] * eta[2] * kappa[0]) * sa \
-            + (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * cb \
-            + (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
-               kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb
-        self.coeffs[1, 7] = -20. * (end_pose[1] - start_pose[1]) + (10. * eta[0] + 2. * eta[2] + 1. / 6 * eta[4]) * sa \
-            + (2. * eta[0]**2 * kappa[0] + 1. / 6 * eta[0]**3 * kappa[1] + 1. / 2 * eta[0] * eta[2] * kappa[0]) * ca \
-            + (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * sb \
-            - (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
-               kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
+        tmp1 = -20. * (end_pose[0] - start_pose[0])
+        tmp2 = (10. * eta[0] + 2. * eta[2] + 1. / 6 * eta[4]) * ca
+        tmp3 = - (2. * eta[0] ** 2 * kappa[0] + 1. / 6 * eta[0] ** 3 * kappa[1]
+                  + 1. / 2 * eta[0] * eta[2] * kappa[0]) * sa
+        tmp4 = (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * cb
+        tmp5 = (2. * eta[1] ** 2 * kappa[2] - 1. / 6 * eta[1] ** 3 * kappa[3]
+                - 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb
+        self.coeffs[0, 7] = tmp1 + tmp2 + tmp3 + tmp4 + tmp5
 
-        self.s_dot = lambda u: max(np.linalg.norm(self.coeffs[:, 1:].dot(np.array(
-            [1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6]))), 1e-6)
+        tmp1 = -20. * (end_pose[1] - start_pose[1])
+        tmp2 = (10. * eta[0] + 2. * eta[2] + 1. / 6 * eta[4]) * sa
+        tmp3 = (2. * eta[0] ** 2 * kappa[0] + 1. / 6 * eta[0] ** 3 * kappa[1]
+                + 1. / 2 * eta[0] * eta[2] * kappa[0]) * ca
+        tmp4 = (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * sb
+        tmp5 = - (2. * eta[1] ** 2 * kappa[2] - 1. / 6 * eta[1] ** 3 * kappa[3]
+                  - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
+        self.coeffs[1, 7] = tmp1 + tmp2 + tmp3 + tmp4 + tmp5
+        self.s_dot = lambda u: max(np.linalg.norm(
+            self.coeffs[:, 1:].dot(np.array(
+                [1, 2. * u, 3. * u**2, 4. * u**3,
+                 5. * u**4, 6. * u**5, 7. * u**6]))), 1e-6)
         self.f_length = lambda ue: quad(lambda u: self.s_dot(u), 0, ue)
         self.segment_length = self.f_length(1)[0]
 
     def calc_point(self, u):
         """
-        eta3_path_segment::calc_point
+        Eta3PathSegment::calc_point
 
         input
             u - parametric representation of a point along the segment, 0 <= u <= 1
         returns
             (x,y) of point along the segment
         """
-        assert(u >= 0 and u <= 1)
+        assert(0 <= u <= 1)
         return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7]))
 
     def calc_deriv(self, u, order=1):
         """
-        eta3_path_segment::calc_deriv
+        Eta3PathSegment::calc_deriv
 
         input
             u - parametric representation of a point along the segment, 0 <= u <= 1
@@ -181,8 +210,8 @@ class eta3_path_segment(object):
             (d^nx/du^n,d^ny/du^n) of point along the segment, for 0 < n <= 2
         """
 
-        assert(u >= 0 and u <= 1)
-        assert(order > 0 and order <= 2)
+        assert(0 <= u <= 1)
+        assert(0 < order <= 2)
         if order == 1:
             return self.coeffs[:, 1:].dot(np.array([1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6]))
 
@@ -199,10 +228,10 @@ def test1():
         # 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(
+        path_segments.append(Eta3PathSegment(
             start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
-        path = eta3_path(path_segments)
+        path = Eta3Path(path_segments)
 
         # interpolate at several points along the path
         ui = np.linspace(0, len(path_segments), 1001)
@@ -214,8 +243,9 @@ def test1():
             # plot the path
             plt.plot(pos[0, :], pos[1, :])
             # for stopping simulation with the esc key.
-            plt.gcf().canvas.mpl_connect('key_release_event',
-                    lambda event: [exit(0) if event.key == 'escape' else None])
+            plt.gcf().canvas.mpl_connect(
+                'key_release_event',
+                lambda event: [exit(0) if event.key == 'escape' else None])
             plt.pause(1.0)
 
     if show_animation:
@@ -232,10 +262,10 @@ def test2():
         # 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(
+        path_segments.append(Eta3PathSegment(
             start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
-        path = eta3_path(path_segments)
+        path = Eta3Path(path_segments)
 
         # interpolate at several points along the path
         ui = np.linspace(0, len(path_segments), 1001)
@@ -261,7 +291,7 @@ def test3():
     # 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 = [4.27, 4.27, 0, 0, 0, 0]
-    path_segments.append(eta3_path_segment(
+    path_segments.append(Eta3PathSegment(
         start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
     # segment 2: line segment
@@ -269,7 +299,7 @@ def test3():
     end_pose = [5.5, 1.5, 0]
     kappa = [0, 0, 0, 0]
     eta = [0, 0, 0, 0, 0, 0]
-    path_segments.append(eta3_path_segment(
+    path_segments.append(Eta3PathSegment(
         start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
     # segment 3: cubic spiral
@@ -277,7 +307,7 @@ def test3():
     end_pose = [7.4377, 1.8235, 0.6667]
     kappa = [0, 0, 1, 1]
     eta = [1.88, 1.88, 0, 0, 0, 0]
-    path_segments.append(eta3_path_segment(
+    path_segments.append(Eta3PathSegment(
         start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
     # segment 4: generic twirl arc
@@ -285,7 +315,7 @@ def test3():
     end_pose = [7.8, 4.3, 1.8]
     kappa = [1, 1, 0.5, 0]
     eta = [7, 10, 10, -10, 4, 4]
-    path_segments.append(eta3_path_segment(
+    path_segments.append(Eta3PathSegment(
         start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
     # segment 5: circular arc
@@ -293,11 +323,11 @@ def test3():
     end_pose = [5.4581, 5.8064, 3.3416]
     kappa = [0.5, 0, 0.5, 0]
     eta = [2.98, 2.98, 0, 0, 0, 0]
-    path_segments.append(eta3_path_segment(
+    path_segments.append(Eta3PathSegment(
         start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
     # construct the whole path
-    path = eta3_path(path_segments)
+    path = Eta3Path(path_segments)
 
     # interpolate at several points along the path
     ui = np.linspace(0, len(path_segments), 1001)
diff --git a/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py b/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
index 22928354..01a68767 100644
--- a/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
+++ b/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
@@ -1,13 +1,14 @@
 """
 
-\eta^3 polynomials trajectory planner
+eta^3 polynomials trajectory planner
 
 author: Joe Dinius, Ph.D (https://jwdinius.github.io)
         Atsushi Sakai (@Atsushi_twi)
 
 Refs:
 - https://jwdinius.github.io/blog/2018/eta3traj
-- [\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots](https://ieeexplore.ieee.org/document/4339545/)
+- [eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots]
+(https://ieeexplore.ieee.org/document/4339545/)
 
 """
 
@@ -19,8 +20,8 @@ import os
 sys.path.append(os.path.relpath("../Eta3SplinePath"))
 
 try:
-    from eta3_spline_path import eta3_path, eta3_path_segment
-except:
+    from eta3_spline_path import Eta3Path, Eta3PathSegment
+except ImportError:
     raise
 
 show_animation = True
@@ -32,7 +33,7 @@ class MaxVelocityNotReached(Exception):
             actual_vel, max_vel)
 
 
-class eta3_trajectory(eta3_path):
+class eta3_trajectory(Eta3Path):
     """
     eta3_trajectory
 
@@ -41,7 +42,7 @@ class eta3_trajectory(eta3_path):
     """
 
     def __init__(self, segments, max_vel, v0=0.0, a0=0.0, max_accel=2.0, max_jerk=5.0):
-       # ensure that all inputs obey the assumptions of the model
+        # ensure that all inputs obey the assumptions of the model
         assert max_vel > 0 and v0 >= 0 and a0 >= 0 and max_accel > 0 and max_jerk > 0 \
             and a0 <= max_accel and v0 <= max_vel
         super(eta3_trajectory, self).__init__(segments=segments)
@@ -61,19 +62,19 @@ class eta3_trajectory(eta3_path):
         self.prev_seg_id = 0
 
     def velocity_profile(self):
-        '''                   /~~~~~----------------~~~~~\
-                             /                            \
-                            /                              \
-                           /                                \
-                          /                                  \
-        (v=v0, a=a0) ~~~~~                                    \
-                                                               \
-                                                                \~~~~~ (vf=0, af=0)
+        r"""                  /~~~~~----------------\
+                             /                       \
+                            /                         \
+                           /                           \
+                          /                             \
+        (v=v0, a=a0) ~~~~~                               \
+                                                          \
+                                                           \ ~~~~~ (vf=0, af=0)
                      pos.|pos.|neg.|   cruise at    |neg.| neg. |neg.
                      max |max.|max.|     max.       |max.| max. |max.
                      jerk|acc.|jerk|    velocity    |jerk| acc. |jerk
             index     0    1    2      3 (optional)   4     5     6
-        '''
+        """
         # delta_a: accel change from initial position to end of maximal jerk section
         delta_a = self.max_accel - self.a0
         # t_s1: time of traversal of maximal jerk section
@@ -112,7 +113,6 @@ class eta3_trajectory(eta3_path):
         self.seg_lengths = np.zeros((7,))
 
         # Section 0: max jerk up to max acceleration
-        index = 0
         self.times[0] = t_s1
         self.vels[0] = v_s1
         self.seg_lengths[0] = s_s1
@@ -195,7 +195,7 @@ class eta3_trajectory(eta3_path):
 
         def fprime(u):
             return self.segments[seg_id].s_dot(u)
-        while (ui >= 0 and ui <= 1) and abs(f(ui)) > tol:
+        while (0 <= ui <= 1) and abs(f(ui)) > tol:
             ui -= f(ui) / fprime(ui)
         ui = max(0, min(ui, 1))
         return ui
@@ -301,7 +301,7 @@ def test1(max_vel=0.5):
         # 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]
-        trajectory_segments.append(eta3_path_segment(
+        trajectory_segments.append(Eta3PathSegment(
             start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
         traj = eta3_trajectory(trajectory_segments,
@@ -335,7 +335,7 @@ def test2(max_vel=0.5):
         # NOTE: INTEGRATOR ERROR EXPLODES WHEN eta[:1] IS ZERO!
         # was: eta = [0, 0, (i - 5) * 20, (5 - i) * 20, 0, 0], now is:
         eta = [0.1, 0.1, (i - 5) * 20, (5 - i) * 20, 0, 0]
-        trajectory_segments.append(eta3_path_segment(
+        trajectory_segments.append(Eta3PathSegment(
             start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
         traj = eta3_trajectory(trajectory_segments,
@@ -366,7 +366,7 @@ def test3(max_vel=2.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 = [4.27, 4.27, 0, 0, 0, 0]
-    trajectory_segments.append(eta3_path_segment(
+    trajectory_segments.append(Eta3PathSegment(
         start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
     # segment 2: line segment
@@ -376,7 +376,7 @@ def test3(max_vel=2.0):
     # NOTE: INTEGRATOR ERROR EXPLODES WHEN eta[:1] IS ZERO!
     # was: eta = [0, 0, 0, 0, 0, 0], now is:
     eta = [0.5, 0.5, 0, 0, 0, 0]
-    trajectory_segments.append(eta3_path_segment(
+    trajectory_segments.append(Eta3PathSegment(
         start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
     # segment 3: cubic spiral
@@ -384,7 +384,7 @@ def test3(max_vel=2.0):
     end_pose = [7.4377, 1.8235, 0.6667]
     kappa = [0, 0, 1, 1]
     eta = [1.88, 1.88, 0, 0, 0, 0]
-    trajectory_segments.append(eta3_path_segment(
+    trajectory_segments.append(Eta3PathSegment(
         start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
     # segment 4: generic twirl arc
@@ -392,7 +392,7 @@ def test3(max_vel=2.0):
     end_pose = [7.8, 4.3, 1.8]
     kappa = [1, 1, 0.5, 0]
     eta = [7, 10, 10, -10, 4, 4]
-    trajectory_segments.append(eta3_path_segment(
+    trajectory_segments.append(Eta3PathSegment(
         start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
     # segment 5: circular arc
@@ -400,7 +400,7 @@ def test3(max_vel=2.0):
     end_pose = [5.4581, 5.8064, 3.3416]
     kappa = [0.5, 0, 0.5, 0]
     eta = [2.98, 2.98, 0, 0, 0, 0]
-    trajectory_segments.append(eta3_path_segment(
+    trajectory_segments.append(Eta3PathSegment(
         start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
 
     # construct the whole path
diff --git a/README.md b/README.md
index 2dcb4bab..ec0d1f74 100644
--- a/README.md
+++ b/README.md
@@ -103,6 +103,8 @@ See this paper for more details:
 - pandas
 
 - [cvxpy](https://www.cvxpy.org/index.html) 
+  
+- pytest (only for unit tests)
 
 # Documentation
 
diff --git a/requirements.txt b/requirements.txt
index c8aa78ae..d1d7a2b0 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -3,3 +3,4 @@ scipy == 1.5.4
 pandas == 1.2.0
 matplotlib == 3.3.3
 cvxpy == 1.1.7
+pytest == 6.2.1
diff --git a/runtests.sh b/runtests.sh
index d7df3cff..d9abc370 100755
--- a/runtests.sh
+++ b/runtests.sh
@@ -1,6 +1,6 @@
 #!/usr/bin/env bash
 echo "Run test suites! "
-export PYTHONWARNINGS=default  # show warning
-#python -m unittest discover tests 
-#python -Wignore -m unittest discover tests #ignore warning
-coverage run -m unittest discover tests # generate coverage file
+# tests: include unittest based tests
+# -Werror: warning as error
+# --durations=0: show ranking of test durations
+pytest tests -Werror --durations=0
diff --git a/tests/__init__.py b/tests/__init__.py
new file mode 100644
index 00000000..e69de29b