Improve B Spline path planner

.github/workflows/pythonpackage.yml

@@ -9,7 +9,7 @@ jobs:
     strategy:
       max-parallel: 4
       matrix:
-        python-version: [3.6, 3.7]
+        python-version: [3.7]
 
     steps:
     - uses: actions/checkout@v1
@@ -29,6 +29,21 @@ jobs:
         # exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide
         flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics
     - name: install coverage
-      run: pip install coverage   
-    - name: Test
+      run: pip install coverage
+    - name: install mypy
+      run: pip install mypy
+    - name: mypy check
+      run: |
+        find AerialNavigation -name "*.py" | xargs mypy
+        find ArmNavigation -name "*.py" | xargs mypy
+        find Bipedal -name "*.py" | xargs mypy
+        find InvertedPendulumCart -name "*.py" | xargs mypy
+        find Localization -name "*.py" | xargs mypy
+        find Mapping -name "*.py" | xargs mypy
+        find PathPlanning -name "*.py" | xargs mypy
+        find PathTracking -name "*.py" | xargs mypy
+        find SLAM -name "*.py" | xargs mypy
+    - name: do all unit tests
       run: bash runtests.sh
+
+

ArmNavigation/n_joint_arm_3d/NLinkArm.py → ArmNavigation/n_joint_arm_3d/NLinkArm3d.py

@@ -7,6 +7,7 @@
 from mpl_toolkits.mplot3d import Axes3D
 import matplotlib.pyplot as plt
 
+
 class Link:
     def __init__(self, dh_params):
         self.dh_params_ = dh_params
@@ -28,16 +29,22 @@ def transformation_matrix(self):
 
         return trans
 
-    def basic_jacobian(self, trans_prev, ee_pos):
-        pos_prev = np.array([trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]])
-        z_axis_prev = np.array([trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]])
+    @staticmethod
+    def basic_jacobian(trans_prev, ee_pos):
+        pos_prev = np.array(
+            [trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]])
+        z_axis_prev = np.array(
+            [trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]])
 
-        basic_jacobian = np.hstack((np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev))
+        basic_jacobian = np.hstack(
+            (np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev))
         return basic_jacobian
-        
+
 
 class NLinkArm:
     def __init__(self, dh_params_list):
+        self.fig = plt.figure()
+        self.ax = Axes3D(self.fig)
         self.link_list = []
         for i in range(len(dh_params_list)):
             self.link_list.append(Link(dh_params_list[i]))
@@ -47,7 +54,7 @@ def transformation_matrix(self):
         for i in range(len(self.link_list)):
             trans = np.dot(trans, self.link_list[i].transformation_matrix())
         return trans
-    
+
     def forward_kinematics(self, plot=False):
         trans = self.transformation_matrix()
 
@@ -57,15 +64,12 @@ def forward_kinematics(self, plot=False):
         alpha, beta, gamma = self.euler_angle()
 
         if plot:
-            self.fig = plt.figure()
-            self.ax = Axes3D(self.fig)
-
             x_list = []
             y_list = []
             z_list = []
-    
+
             trans = np.identity(4)
-            
+
             x_list.append(trans[0, 3])
             y_list.append(trans[1, 3])
             z_list.append(trans[2, 3])
@@ -74,56 +78,61 @@ def forward_kinematics(self, plot=False):
                 x_list.append(trans[0, 3])
                 y_list.append(trans[1, 3])
                 z_list.append(trans[2, 3])
-
-            self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
+
+            self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4,
+                         mew=0.5)
             self.ax.plot([0], [0], [0], "o")
-            
+
             self.ax.set_xlim(-1, 1)
             self.ax.set_ylim(-1, 1)
             self.ax.set_zlim(-1, 1)
-  
+
             plt.show()
 
         return [x, y, z, alpha, beta, gamma]
 
     def basic_jacobian(self):
         ee_pos = self.forward_kinematics()[0:3]
         basic_jacobian_mat = []
-        
-        trans = np.identity(4)        
+
+        trans = np.identity(4)
         for i in range(len(self.link_list)):
-            basic_jacobian_mat.append(self.link_list[i].basic_jacobian(trans, ee_pos))
+            basic_jacobian_mat.append(
+                self.link_list[i].basic_jacobian(trans, ee_pos))
             trans = np.dot(trans, self.link_list[i].transformation_matrix())
-            
+
         return np.array(basic_jacobian_mat).T
 
     def inverse_kinematics(self, ref_ee_pose, plot=False):
         for cnt in range(500):
             ee_pose = self.forward_kinematics()
             diff_pose = np.array(ref_ee_pose) - ee_pose
-        
+
             basic_jacobian_mat = self.basic_jacobian()
             alpha, beta, gamma = self.euler_angle()
-
-            K_zyz = np.array([[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
-                              [0, math.cos(alpha), math.sin(alpha) * math.sin(beta)],
-                              [1, 0, math.cos(beta)]])
+
+            K_zyz = np.array(
+                [[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
+                 [0, math.cos(alpha), math.sin(alpha) * math.sin(beta)],
+                 [1, 0, math.cos(beta)]])
             K_alpha = np.identity(6)
             K_alpha[3:, 3:] = K_zyz
 
-            theta_dot = np.dot(np.dot(np.linalg.pinv(basic_jacobian_mat), K_alpha), np.array(diff_pose))
+            theta_dot = np.dot(
+                np.dot(np.linalg.pinv(basic_jacobian_mat), K_alpha),
+                np.array(diff_pose))
             self.update_joint_angles(theta_dot / 100.)
-            
+
         if plot:
             self.fig = plt.figure()
             self.ax = Axes3D(self.fig)
-            
+
             x_list = []
             y_list = []
             z_list = []
-    
+
             trans = np.identity(4)
-            
+
             x_list.append(trans[0, 3])
             y_list.append(trans[1, 3])
             z_list.append(trans[2, 3])
@@ -132,48 +141,54 @@ def inverse_kinematics(self, ref_ee_pose, plot=False):
                 x_list.append(trans[0, 3])
                 y_list.append(trans[1, 3])
                 z_list.append(trans[2, 3])
-
-            self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
+
+            self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4,
+                         mew=0.5)
             self.ax.plot([0], [0], [0], "o")
-            
+
             self.ax.set_xlim(-1, 1)
             self.ax.set_ylim(-1, 1)
             self.ax.set_zlim(-1, 1)
-
-            self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]], "o")
+
+            self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]],
+                         "o")
             plt.show()
-                
+
     def euler_angle(self):
         trans = self.transformation_matrix()
-        
+
         alpha = math.atan2(trans[1][2], trans[0][2])
-        if not (-math.pi / 2 <= alpha and alpha <= math.pi / 2):
+        if not (-math.pi / 2 <= alpha <= math.pi / 2):
             alpha = math.atan2(trans[1][2], trans[0][2]) + math.pi
-        if not (-math.pi / 2 <= alpha and alpha <= math.pi / 2):
+        if not (-math.pi / 2 <= alpha <= math.pi / 2):
             alpha = math.atan2(trans[1][2], trans[0][2]) - math.pi
-        beta = math.atan2(trans[0][2] * math.cos(alpha) + trans[1][2] * math.sin(alpha), trans[2][2])
-        gamma = math.atan2(-trans[0][0] * math.sin(alpha) + trans[1][0] * math.cos(alpha), -trans[0][1] * math.sin(alpha) + trans[1][1] * math.cos(alpha))
-
+        beta = math.atan2(
+            trans[0][2] * math.cos(alpha) + trans[1][2] * math.sin(alpha),
+            trans[2][2])
+        gamma = math.atan2(
+            -trans[0][0] * math.sin(alpha) + trans[1][0] * math.cos(alpha),
+            -trans[0][1] * math.sin(alpha) + trans[1][1] * math.cos(alpha))
+
         return alpha, beta, gamma
-    
+
     def set_joint_angles(self, joint_angle_list):
         for i in range(len(self.link_list)):
             self.link_list[i].dh_params_[0] = joint_angle_list[i]
 
     def update_joint_angles(self, diff_joint_angle_list):
         for i in range(len(self.link_list)):
             self.link_list[i].dh_params_[0] += diff_joint_angle_list[i]
-        
+
     def plot(self):
         self.fig = plt.figure()
         self.ax = Axes3D(self.fig)
-        
+
         x_list = []
         y_list = []
         z_list = []
 
         trans = np.identity(4)
-        
+
         x_list.append(trans[0, 3])
         y_list.append(trans[1, 3])
         z_list.append(trans[2, 3])
@@ -182,15 +197,16 @@ def plot(self):
             x_list.append(trans[0, 3])
             y_list.append(trans[1, 3])
             z_list.append(trans[2, 3])
-
-        self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
+
+        self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4,
+                     mew=0.5)
         self.ax.plot([0], [0], [0], "o")
 
         self.ax.set_xlabel("x")
         self.ax.set_ylabel("y")
-        self.ax.set_zlabel("z")        
-        
+        self.ax.set_zlabel("z")
+
         self.ax.set_xlim(-1, 1)
         self.ax.set_ylim(-1, 1)
-        self.ax.set_zlim(-1, 1)        
+        self.ax.set_zlim(-1, 1)
         plt.show()

ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py

@@ -3,9 +3,10 @@
 Author: Takayuki Murooka (takayuki5168)
 """
 import math
-from NLinkArm import NLinkArm
+from NLinkArm3d import NLinkArm
 import random
 
+
 def random_val(min_val, max_val):
     return min_val + random.random() * (max_val - min_val)
 
@@ -14,17 +15,17 @@ def random_val(min_val, max_val):
     print("Start solving Forward Kinematics 10 times")
 
     # init NLinkArm with Denavit-Hartenberg parameters of PR2    
-    n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.],
-                           [math.pi/2, math.pi/2, 0., 0.],
-                           [0., -math.pi/2, 0., .4],
-                           [0., math.pi/2, 0., 0.],
-                           [0., -math.pi/2, 0., .321],
-                           [0., math.pi/2, 0., 0.],
+    n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.],
+                           [math.pi / 2, math.pi / 2, 0., 0.],
+                           [0., -math.pi / 2, 0., .4],
+                           [0., math.pi / 2, 0., 0.],
+                           [0., -math.pi / 2, 0., .321],
+                           [0., math.pi / 2, 0., 0.],
                            [0., 0., 0., 0.]])
 
     # execute FK 10 times
     for i in range(10):
-        n_link_arm.set_joint_angles([random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
-
-        ee_pose = n_link_arm.forward_kinematics(plot=True)
+        n_link_arm.set_joint_angles(
+            [random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
 
+        ee_pose = n_link_arm.forward_kinematics(plot=True)

ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py

@@ -3,23 +3,24 @@
 Author: Takayuki Murooka (takayuki5168)
 """
 import math
-from NLinkArm import NLinkArm
+from NLinkArm3d import NLinkArm
 import random
 
 
 def random_val(min_val, max_val):
     return min_val + random.random() * (max_val - min_val)
 
+
 if __name__ == "__main__":
     print("Start solving Inverse Kinematics 10 times")
 
     # init NLinkArm with Denavit-Hartenberg parameters of PR2        
-    n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.],
-                           [math.pi/2, math.pi/2, 0., 0.],
-                           [0., -math.pi/2, 0., .4],
-                           [0., math.pi/2, 0., 0.],
-                           [0., -math.pi/2, 0., .321],
-                           [0., math.pi/2, 0., 0.],
+    n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.],
+                           [math.pi / 2, math.pi / 2, 0., 0.],
+                           [0., -math.pi / 2, 0., .4],
+                           [0., math.pi / 2, 0., 0.],
+                           [0., -math.pi / 2, 0., .321],
+                           [0., math.pi / 2, 0., 0.],
                            [0., 0., 0., 0.]])
 
     # execute IK 10 times

PathPlanning/AStar/a_star.py

@@ -41,7 +41,8 @@ def __init__(self, x, y, cost, pind):
             self.pind = pind
 
         def __str__(self):
-            return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
+            return str(self.x) + "," + str(self.y) + "," + str(
+                self.cost) + "," + str(self.pind)
 
     def planning(self, sx, sy, gx, gy):
         """
@@ -72,7 +73,10 @@ def planning(self, sx, sy, gx, gy):
                 break
 
             c_id = min(
-                open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, open_set[o]))
+                open_set,
+                key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal,
+                                                                     open_set[
+                                                                         o]))
             current = open_set[c_id]
 
             # show graph
@@ -81,7 +85,8 @@ def planning(self, sx, sy, gx, gy):
                          self.calc_grid_position(current.y, self.miny), "xc")
                 # 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])
+                                             lambda event: [exit(
+                                                 0) if event.key == 'escape' else None])
                 if len(closed_set.keys()) % 10 == 0:
                     plt.pause(0.001)
 
@@ -104,7 +109,6 @@ def planning(self, sx, sy, gx, gy):
                                  current.cost + self.motion[i][2], c_id)
                 n_id = self.calc_grid_index(node)
 
-
                 # If the node is not safe, do nothing
                 if not self.verify_node(node):
                     continue