From 2d90bd5a52cdd58b635e95ec5d5ef3d59a239caf Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Fri, 25 Jan 2019 02:01:51 +0900
Subject: [PATCH 01/29] add n_joint_arm_3d & implement forward kinematics

---
 ArmNavigation/n_joint_arm_3d/NLinkArm.py      | 94 +++++++++++++++++++
 .../n_joint_arm_3d/random_joint_angles.py     | 20 ++++
 2 files changed, 114 insertions(+)
 create mode 100644 ArmNavigation/n_joint_arm_3d/NLinkArm.py
 create mode 100644 ArmNavigation/n_joint_arm_3d/random_joint_angles.py

diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
new file mode 100644
index 00000000..d50f9d5c
--- /dev/null
+++ b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
@@ -0,0 +1,94 @@
+import numpy as np
+import math
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plt
+
+class Link:
+    def __init__(self, dh_params):
+        self.dh_params_ = dh_params
+
+    def calc_transformation_matrix(self):
+        theta = self.dh_params_[0]
+        alpha = self.dh_params_[1]
+        a = self.dh_params_[2]
+        d = self.dh_params_[3]
+
+        trans = np.array(
+            [[math.cos(theta), -math.sin(theta), 0, a],
+             [math.cos(alpha) * math.sin(theta), math.cos(alpha) * math.cos(theta), -math.sin(alpha), -d * math.sin(alpha)],
+             [math.sin(alpha) * math.sin(theta), math.sin(alpha) * math.cos(theta), math.cos(alpha), d * math.cos(alpha)],
+             [0, 0, 0, 1]])
+
+        return trans
+
+class NLinkArm:
+    def __init__(self, dh_params_list):
+        self.link_list = []
+        for i in range(len(dh_params_list)):
+            self.link_list.append(Link(dh_params_list[i]))
+
+        self.fig = plt.figure()
+        self.ax = Axes3D(self.fig)
+
+    def calc_transformation_matrix(self):
+        trans = np.array([[1, 0, 0, 0],
+                             [0, 1, 0, 0],
+                             [0, 0, 1, 0],
+                             [0, 0, 0, 1]])
+        for i in range(len(self.link_list)):
+            trans = np.dot(trans, self.link_list[i].calc_transformation_matrix())
+            
+        return trans
+    
+    def forward_kinematics(self):
+        trans = self.calc_transformation_matrix()
+
+        x = trans[0, 3]
+        y = trans[1, 3]
+        z = trans[2, 3]
+        alpha = math.atan2(trans[1, 2], trans[1, 3])
+        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 [x, y, z, 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 plot(self):
+        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])
+        for i in range(len(self.link_list)):
+            trans = np.dot(trans, self.link_list[i].calc_transformation_matrix())
+            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([0], [0], [0], "o")
+        
+        self.ax.set_xlim(-1, 1)
+        self.ax.set_ylim(-1, 1)
+        self.ax.set_zlim(-1, 1)        
+        plt.show()
+        
+if __name__ == "__main__":
+    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.]])
+
+    print(n_link_arm.forward_kinematics())
+    n_link_arm.set_joint_angles([1, 1, 1, 1, 1, 1, 1])
+    n_link_arm.plot()
diff --git a/ArmNavigation/n_joint_arm_3d/random_joint_angles.py b/ArmNavigation/n_joint_arm_3d/random_joint_angles.py
new file mode 100644
index 00000000..54be155e
--- /dev/null
+++ b/ArmNavigation/n_joint_arm_3d/random_joint_angles.py
@@ -0,0 +1,20 @@
+import math
+from NLinkArm import NLinkArm
+import random
+import time
+
+def random_val(min_val, max_val):
+    return min_val + random.random() * (max_val - min_val)
+
+for i in range(10):
+    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.]])
+
+    n_link_arm.set_joint_angles([random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
+    n_link_arm.plot()
+

From c49f982923cb415e80291ae7d743fd42532713d8 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Fri, 25 Jan 2019 21:39:32 +0900
Subject: [PATCH 02/29] implement inverse kinematics

---
 ArmNavigation/n_joint_arm_3d/NLinkArm.py      | 65 ++++++++++++++++---
 ...angles.py => random_forward_kinematics.py} |  0
 .../random_inverse_kinematics.py              | 17 +++++
 3 files changed, 72 insertions(+), 10 deletions(-)
 rename ArmNavigation/n_joint_arm_3d/{random_joint_angles.py => random_forward_kinematics.py} (100%)
 create mode 100644 ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py

diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
index d50f9d5c..f6b9c287 100644
--- a/ArmNavigation/n_joint_arm_3d/NLinkArm.py
+++ b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
@@ -7,7 +7,7 @@ class Link:
     def __init__(self, dh_params):
         self.dh_params_ = dh_params
 
-    def calc_transformation_matrix(self):
+    def transformation_matrix(self):
         theta = self.dh_params_[0]
         alpha = self.dh_params_[1]
         a = self.dh_params_[2]
@@ -21,6 +21,14 @@ class Link:
 
         return trans
 
+    def basic_jacobian(self, trans_prev, ee_pose):
+        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_pose - pos_prev), z_axis_prev))
+        return basic_jacobian
+        
+
 class NLinkArm:
     def __init__(self, dh_params_list):
         self.link_list = []
@@ -30,18 +38,14 @@ class NLinkArm:
         self.fig = plt.figure()
         self.ax = Axes3D(self.fig)
 
-    def calc_transformation_matrix(self):
-        trans = np.array([[1, 0, 0, 0],
-                             [0, 1, 0, 0],
-                             [0, 0, 1, 0],
-                             [0, 0, 0, 1]])
+    def transformation_matrix(self):
+        trans = np.identity(4)
         for i in range(len(self.link_list)):
-            trans = np.dot(trans, self.link_list[i].calc_transformation_matrix())
-            
+            trans = np.dot(trans, self.link_list[i].transformation_matrix())
         return trans
     
     def forward_kinematics(self):
-        trans = self.calc_transformation_matrix()
+        trans = self.transformation_matrix()
 
         x = trans[0, 3]
         y = trans[1, 3]
@@ -52,9 +56,50 @@ class NLinkArm:
 
         return [x, y, z, alpha, beta, gamma]
 
+    def basic_jacobian(self, ref_ee_pose):
+        basic_jacobian_mat = []
+        
+        trans = np.identity(4)        
+        for i in range(len(self.link_list)):
+            trans = np.dot(trans, self.link_list[i].transformation_matrix())
+            basic_jacobian_mat.append(self.link_list[i].basic_jacobian(trans, ref_ee_pose[0:3]))
+            
+        #print(np.array(basic_jacobian_mat).T)
+        return np.array(basic_jacobian_mat).T
+
+    def inverse_kinematics(self, ref_ee_pose):
+        ee_pose = self.forward_kinematics()
+        diff_pose = ee_pose - np.array(ref_ee_pose)
+        
+        for cnt in range(1000):
+            basic_jacobian_mat = self.basic_jacobian(ref_ee_pose)
+            alpha, beta, gamma = self.calc_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_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))
+            self.update_joint_angles(theta_dot)
+
+    def calc_euler_angle(self):
+        trans = self.transformation_matrix()
+        
+        alpha = math.atan2(trans[1][2], trans[0][2])
+        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):
         x_list = []
@@ -67,7 +112,7 @@ class NLinkArm:
         y_list.append(trans[1, 3])
         z_list.append(trans[2, 3])
         for i in range(len(self.link_list)):
-            trans = np.dot(trans, self.link_list[i].calc_transformation_matrix())
+            trans = np.dot(trans, self.link_list[i].transformation_matrix())
             x_list.append(trans[0, 3])
             y_list.append(trans[1, 3])
             z_list.append(trans[2, 3])
diff --git a/ArmNavigation/n_joint_arm_3d/random_joint_angles.py b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
similarity index 100%
rename from ArmNavigation/n_joint_arm_3d/random_joint_angles.py
rename to ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
diff --git a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py
new file mode 100644
index 00000000..a7286475
--- /dev/null
+++ b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py
@@ -0,0 +1,17 @@
+import math
+from NLinkArm import NLinkArm
+import random
+import time
+
+
+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.]])
+
+#n_link_arm.inverse_kinematics([-0.621, 0., 0., 0., 0., math.pi / 2])
+n_link_arm.inverse_kinematics([-0.5, 0., 0.1, 0., 0., math.pi / 2])
+n_link_arm.plot()

From 3504833748d7e5ebb0d9b8ca82b4ab854bd25572 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Sat, 26 Jan 2019 02:23:53 +0900
Subject: [PATCH 03/29] fix bug of NLinkArm.py and modify random exapmle
 scripts

---
 ArmNavigation/n_joint_arm_3d/NLinkArm.py      | 106 +++++++++++++++---
 .../random_forward_kinematics.py              |  16 ++-
 .../random_inverse_kinematics.py              |  32 ++++--
 3 files changed, 121 insertions(+), 33 deletions(-)

diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
index f6b9c287..e1d73697 100644
--- a/ArmNavigation/n_joint_arm_3d/NLinkArm.py
+++ b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
@@ -13,11 +13,21 @@ class Link:
         a = self.dh_params_[2]
         d = self.dh_params_[3]
 
+        '''
         trans = np.array(
             [[math.cos(theta), -math.sin(theta), 0, a],
              [math.cos(alpha) * math.sin(theta), math.cos(alpha) * math.cos(theta), -math.sin(alpha), -d * math.sin(alpha)],
              [math.sin(alpha) * math.sin(theta), math.sin(alpha) * math.cos(theta), math.cos(alpha), d * math.cos(alpha)],
              [0, 0, 0, 1]])
+        '''
+        st = math.sin(theta)
+        ct = math.cos(theta)
+        sa = math.sin(alpha)
+        ca = math.cos(alpha)
+        trans = np.array([[ct, -st * ca, st * sa, a * ct],
+                          [st, ct * ca, -ct * sa, a * st],
+                          [0, sa, ca, d],
+                          [0, 0, 0, 1]])
 
         return trans
 
@@ -35,44 +45,66 @@ class NLinkArm:
         for i in range(len(dh_params_list)):
             self.link_list.append(Link(dh_params_list[i]))
 
-        self.fig = plt.figure()
-        self.ax = Axes3D(self.fig)
-
     def transformation_matrix(self):
         trans = np.identity(4)
         for i in range(len(self.link_list)):
             trans = np.dot(trans, self.link_list[i].transformation_matrix())
         return trans
     
-    def forward_kinematics(self):
+    def forward_kinematics(self, plot=False):
         trans = self.transformation_matrix()
 
         x = trans[0, 3]
         y = trans[1, 3]
         z = trans[2, 3]
-        alpha = math.atan2(trans[1, 2], trans[1, 3])
-        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))
+        alpha, beta, gamma = self.calc_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])
+            for i in range(len(self.link_list)):
+                trans = np.dot(trans, self.link_list[i].transformation_matrix())
+                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([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, ref_ee_pose):
+    def basic_jacobian(self, ee_pose):
         basic_jacobian_mat = []
         
         trans = np.identity(4)        
         for i in range(len(self.link_list)):
+            basic_jacobian_mat.append(self.link_list[i].basic_jacobian(trans, ee_pose[0:3]))
             trans = np.dot(trans, self.link_list[i].transformation_matrix())
-            basic_jacobian_mat.append(self.link_list[i].basic_jacobian(trans, ref_ee_pose[0:3]))
             
-        #print(np.array(basic_jacobian_mat).T)
         return np.array(basic_jacobian_mat).T
 
-    def inverse_kinematics(self, ref_ee_pose):
-        ee_pose = self.forward_kinematics()
-        diff_pose = ee_pose - np.array(ref_ee_pose)
+    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
         
-        for cnt in range(1000):
-            basic_jacobian_mat = self.basic_jacobian(ref_ee_pose)
+            basic_jacobian_mat = self.basic_jacobian(ee_pose)
             alpha, beta, gamma = self.calc_euler_angle()
             
             K_zyz = np.array([[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
@@ -82,12 +114,45 @@ class NLinkArm:
             K_alpha[3:, 3:] = K_zyz
 
             theta_dot = np.dot(np.dot(np.linalg.pinv(basic_jacobian_mat), K_alpha), np.array(diff_pose))
-            self.update_joint_angles(theta_dot)
-
+            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])
+            for i in range(len(self.link_list)):
+                trans = np.dot(trans, self.link_list[i].transformation_matrix())
+                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([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")
+            plt.show()
+                
     def calc_euler_angle(self):
         trans = self.transformation_matrix()
         
         alpha = math.atan2(trans[1][2], trans[0][2])
+        if -math.pi / 2 <= alpha and alpha <= math.pi / 2:
+            alpha = math.atan2(trans[1][2], trans[0][2]) + math.pi
+        if -math.pi / 2 <= alpha and 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))
         
@@ -102,6 +167,9 @@ class NLinkArm:
             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 = []
@@ -119,6 +187,10 @@ class NLinkArm:
             
         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_xlim(-1, 1)
         self.ax.set_ylim(-1, 1)
diff --git a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
index 54be155e..df037fb5 100644
--- a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
+++ b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
@@ -1,12 +1,15 @@
 import math
 from NLinkArm import NLinkArm
 import random
-import time
 
 def random_val(min_val, max_val):
     return min_val + random.random() * (max_val - min_val)
 
-for i in range(10):
+
+if __name__ == "__main__":
+    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],
@@ -14,7 +17,10 @@ for i in range(10):
                            [0., -math.pi/2, 0., .321],
                            [0., math.pi/2, 0., 0.],
                            [0., 0., 0., 0.]])
-
-    n_link_arm.set_joint_angles([random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
-    n_link_arm.plot()
+    
+    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)
+        print(ee_pose)
 
diff --git a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py
index a7286475..dd513816 100644
--- a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py
+++ b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py
@@ -1,17 +1,27 @@
 import math
 from NLinkArm import NLinkArm
 import random
-import time
 
 
-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.]])
+def random_val(min_val, max_val):
+    return min_val + random.random() * (max_val - min_val)
 
-#n_link_arm.inverse_kinematics([-0.621, 0., 0., 0., 0., math.pi / 2])
-n_link_arm.inverse_kinematics([-0.5, 0., 0.1, 0., 0., math.pi / 2])
-n_link_arm.plot()
+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.],
+                           [0., 0., 0., 0.]])
+
+    for i in range(10):
+        n_link_arm.inverse_kinematics([random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5),
+                                       random_val(-0.5, 0.5)], plot=True)

From b543623cada7f6f425df6463b44027fdc4ec211d Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Sat, 26 Jan 2019 02:32:49 +0900
Subject: [PATCH 04/29] minor change

---
 ArmNavigation/n_joint_arm_3d/NLinkArm.py | 20 --------------------
 1 file changed, 20 deletions(-)

diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
index e1d73697..19b1a32a 100644
--- a/ArmNavigation/n_joint_arm_3d/NLinkArm.py
+++ b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
@@ -13,13 +13,6 @@ class Link:
         a = self.dh_params_[2]
         d = self.dh_params_[3]
 
-        '''
-        trans = np.array(
-            [[math.cos(theta), -math.sin(theta), 0, a],
-             [math.cos(alpha) * math.sin(theta), math.cos(alpha) * math.cos(theta), -math.sin(alpha), -d * math.sin(alpha)],
-             [math.sin(alpha) * math.sin(theta), math.sin(alpha) * math.cos(theta), math.cos(alpha), d * math.cos(alpha)],
-             [0, 0, 0, 1]])
-        '''
         st = math.sin(theta)
         ct = math.cos(theta)
         sa = math.sin(alpha)
@@ -196,16 +189,3 @@ class NLinkArm:
         self.ax.set_ylim(-1, 1)
         self.ax.set_zlim(-1, 1)        
         plt.show()
-        
-if __name__ == "__main__":
-    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.]])
-
-    print(n_link_arm.forward_kinematics())
-    n_link_arm.set_joint_angles([1, 1, 1, 1, 1, 1, 1])
-    n_link_arm.plot()

From 33e7b67db0130f6ec5277fdf7e630738d13fb2dd Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Sat, 26 Jan 2019 03:06:27 +0900
Subject: [PATCH 05/29] minor changes

---
 ArmNavigation/n_joint_arm_3d/NLinkArm.py      | 21 ++++++++++---------
 .../random_forward_kinematics.py              |  1 -
 2 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
index 19b1a32a..d0e32a05 100644
--- a/ArmNavigation/n_joint_arm_3d/NLinkArm.py
+++ b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
@@ -24,11 +24,11 @@ class Link:
 
         return trans
 
-    def basic_jacobian(self, trans_prev, ee_pose):
+    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]])
 
-        basic_jacobian = np.hstack((np.cross(z_axis_prev, ee_pose - pos_prev), z_axis_prev))
+        basic_jacobian = np.hstack((np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev))
         return basic_jacobian
         
 
@@ -50,7 +50,7 @@ class NLinkArm:
         x = trans[0, 3]
         y = trans[1, 3]
         z = trans[2, 3]
-        alpha, beta, gamma = self.calc_euler_angle()
+        alpha, beta, gamma = self.euler_angle()
 
         if plot:
             self.fig = plt.figure()
@@ -82,12 +82,13 @@ class NLinkArm:
 
         return [x, y, z, alpha, beta, gamma]
 
-    def basic_jacobian(self, ee_pose):
+    def basic_jacobian(self):
+        ee_pos = self.forward_kinematics()[0:3]
         basic_jacobian_mat = []
         
         trans = np.identity(4)        
         for i in range(len(self.link_list)):
-            basic_jacobian_mat.append(self.link_list[i].basic_jacobian(trans, ee_pose[0:3]))
+            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
@@ -97,8 +98,8 @@ class NLinkArm:
             ee_pose = self.forward_kinematics()
             diff_pose = np.array(ref_ee_pose) - ee_pose
         
-            basic_jacobian_mat = self.basic_jacobian(ee_pose)
-            alpha, beta, gamma = self.calc_euler_angle()
+            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)],
@@ -138,13 +139,13 @@ class NLinkArm:
             self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]], "o")
             plt.show()
                 
-    def calc_euler_angle(self):
+    def euler_angle(self):
         trans = self.transformation_matrix()
         
         alpha = math.atan2(trans[1][2], trans[0][2])
-        if -math.pi / 2 <= alpha and alpha <= math.pi / 2:
+        if not (-math.pi / 2 <= alpha and alpha <= math.pi / 2):
             alpha = math.atan2(trans[1][2], trans[0][2]) + math.pi
-        if -math.pi / 2 <= alpha and alpha <= math.pi / 2:
+        if not (-math.pi / 2 <= alpha and 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))
diff --git a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
index df037fb5..e9479521 100644
--- a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
+++ b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
@@ -22,5 +22,4 @@ if __name__ == "__main__":
         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)
-        print(ee_pose)
 

From 47b497019a0bcedc2cd257ea8bd48942f2b616d5 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Sat, 26 Jan 2019 15:28:59 +0900
Subject: [PATCH 06/29] add header comment in each scripts

---
 ArmNavigation/n_joint_arm_3d/NLinkArm.py                  | 4 ++++
 ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py | 7 ++++++-
 ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py | 5 +++++
 3 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
index d0e32a05..da562d9f 100644
--- a/ArmNavigation/n_joint_arm_3d/NLinkArm.py
+++ b/ArmNavigation/n_joint_arm_3d/NLinkArm.py
@@ -1,3 +1,7 @@
+"""
+Class of n-link arm in 3D
+Author: Takayuki Murooka (takayuki5168)
+"""
 import numpy as np
 import math
 from mpl_toolkits.mplot3d import Axes3D
diff --git a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
index e9479521..87907f80 100644
--- a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
+++ b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
@@ -1,3 +1,7 @@
+"""
+Forward Kinematics for an n-link arm in 3D
+Author: Takayuki Murooka (takayuki5168)
+"""
 import math
 from NLinkArm import NLinkArm
 import random
@@ -17,7 +21,8 @@ if __name__ == "__main__":
                            [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))])
         
diff --git a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py
index dd513816..44be2270 100644
--- a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py
+++ b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py
@@ -1,3 +1,7 @@
+"""
+Inverse Kinematics for an n-link arm in 3D
+Author: Takayuki Murooka (takayuki5168)
+"""
 import math
 from NLinkArm import NLinkArm
 import random
@@ -18,6 +22,7 @@ if __name__ == "__main__":
                            [0., math.pi/2, 0., 0.],
                            [0., 0., 0., 0.]])
 
+    # execute IK 10 times
     for i in range(10):
         n_link_arm.inverse_kinematics([random_val(-0.5, 0.5),
                                        random_val(-0.5, 0.5),

From c1ba9bab0a48100aa3f81286a2015c5112bb1f9f Mon Sep 17 00:00:00 2001
From: Francisco Moretti <franciscoemoretti@gmail.com>
Date: Thu, 17 Oct 2019 09:09:54 -0300
Subject: [PATCH 07/29] fix check segment intersection with obstacle

The previous method only worked because the step that checked collision
from a point of the line to obstacles was the same as the minimum
obstacle radius. If the obstacle radius is very small a great amount of
steps would be required.
Thus It's better to check the distance from the segment to the obstacles
directly and compare with obstacle radius
Now there is no need to have two check functions.
---
 .../InformedRRTStar/informed_rrt_star.py      | 52 ++++++++++---------
 1 file changed, 28 insertions(+), 24 deletions(-)

diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py
index 4a32801d..b9769bea 100644
--- a/PathPlanning/InformedRRTStar/informed_rrt_star.py
+++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py
@@ -74,10 +74,9 @@ class InformedRRTStar:
             newNode = self.get_new_node(theta, nind, nearestNode)
             d = self.line_cost(nearestNode, newNode)
 
-            isCollision = self.collision_check(newNode, self.obstacle_list)
-            isCollisionEx = self.check_collision_extend(nearestNode, theta, d)
+            isCollision = self.check_collision(nearestNode, theta, d)
 
-            if isCollision and isCollisionEx:
+            if isCollision:
                 nearInds = self.find_near_nodes(newNode)
                 newNode = self.choose_parent(newNode, nearInds)
 
@@ -110,7 +109,7 @@ class InformedRRTStar:
             dy = newNode.y - self.node_list[i].y
             d = math.sqrt(dx ** 2 + dy ** 2)
             theta = math.atan2(dy, dx)
-            if self.check_collision_extend(self.node_list[i], theta, d):
+            if self.check_collision(self.node_list[i], theta, d):
                 dList.append(self.node_list[i].cost + d)
             else:
                 dList.append(float('inf'))
@@ -194,17 +193,6 @@ class InformedRRTStar:
         minIndex = dList.index(min(dList))
         return minIndex
 
-    @staticmethod
-    def collision_check(newNode, obstacleList):
-        for (ox, oy, size) in obstacleList:
-            dx = ox - newNode.x
-            dy = oy - newNode.y
-            d = dx * dx + dy * dy
-            if d <= 1.1 * size ** 2:
-                return False  # collision
-
-        return True  # safe
-
     def get_new_node(self, theta, nind, nearestNode):
         newNode = copy.deepcopy(nearestNode)
 
@@ -234,19 +222,35 @@ class InformedRRTStar:
             if nearNode.cost > scost:
                 theta = math.atan2(newNode.y - nearNode.y,
                                    newNode.x - nearNode.x)
-                if self.check_collision_extend(nearNode, theta, d):
+                if self.check_collision(nearNode, theta, d):
                     nearNode.parent = n_node - 1
                     nearNode.cost = scost
+                    
+    @staticmethod
+    def distance_squared_point_to_segment(v, w, p):
+        # Return minimum distance between line segment vw and point p
+        if (np.array_equal(v, w)):
+            return (p-v).dot(p-v) # v == w case
+        l2 = (w-v).dot(w-v) # i.e. |w-v|^2 -  avoid a sqrt
+        # Consider the line extending the segment, parameterized as v + t (w - v).
+        # We find projection of point p onto the line. 
+        # It falls where t = [(p-v) . (w-v)] / |w-v|^2
+        # We clamp t from [0,1] to handle points outside the segment vw.
+        t = max(0, min(1, (p - v).dot(w - v) / l2))
+        projection = v + t * (w - v) # Projection falls on the segment
+        return (p-projection).dot(p-projection)
 
-    def check_collision_extend(self, nearNode, theta, d):
+    def check_collision(self, nearNode, theta, d):
         tmpNode = copy.deepcopy(nearNode)
-
-        for i in range(int(d / self.expand_dis)):
-            tmpNode.x += self.expand_dis * math.cos(theta)
-            tmpNode.y += self.expand_dis * math.sin(theta)
-            if not self.collision_check(tmpNode, self.obstacle_list):
-                return False
-
+        endx = tmpNode.x + math.cos(theta)*d
+        endy = tmpNode.y + math.sin(theta)*d
+        for (ox, oy, size) in self.obstacle_list:
+            dd = self.distance_squared_point_to_segment(
+                np.array([tmpNode.x, tmpNode.y]),
+                np.array([endx, endy]),
+                np.array([ox, oy]))
+            if dd <= 1.1 * size**2:
+                return False  # collision
         return True
 
     def get_final_course(self, lastIndex):

From b2d65f277ec9d7a122baaec7d2bf4a4b77c5d6a8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?=
 <20567087+goktug97@users.noreply.github.com>
Date: Sat, 26 Oct 2019 09:11:23 +0300
Subject: [PATCH 08/29] change resampling function to match with the book

- changed resampling function so that it chooses random number once for the starting point.
- condition check is moved from the inside of the resampling function
to the outside of the function, this way the purpose of the
function is much clear. Instead "resampling" every iteration,
resample if the condition holds.
---
 .../particle_filter/particle_filter.py        | 30 +++++++++----------
 1 file changed, 14 insertions(+), 16 deletions(-)

diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py
index 6643116f..0ebb5fc3 100644
--- a/Localization/particle_filter/particle_filter.py
+++ b/Localization/particle_filter/particle_filter.py
@@ -128,8 +128,9 @@ def pf_localization(px, pw, z, u):
     xEst = px.dot(pw.T)
     PEst = calc_covariance(xEst, px, pw)
 
-    px, pw = re_sampling(px, pw)
-
+    N_eff = 1.0 / (pw.dot(pw.T))[0, 0]  # Effective particle number
+    if N_eff < NTh:
+        px, pw = re_sampling(px, pw)
     return xEst, PEst, px, pw
 
 
@@ -138,21 +139,18 @@ def re_sampling(px, pw):
     low variance re-sampling
     """
 
-    N_eff = 1.0 / (pw.dot(pw.T))[0, 0]  # Effective particle number
-    if N_eff < NTh:
-        w_cum = np.cumsum(pw)
-        base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
-        re_sample_id = base + np.random.rand(base.shape[0]) / NP
+    w_cum = np.cumsum(pw)
+    base = np.arange(0.0, 1.0, 1/NP)
+    re_sample_id = base + np.random.uniform(0, 1/NP)
+    indexes = []
+    ind = 0
+    for ip in range(NP):
+        while re_sample_id[ip] > w_cum[ind]:
+            ind += 1
+        indexes.append(ind)
 
-        indexes = []
-        ind = 0
-        for ip in range(NP):
-            while re_sample_id[ip] > w_cum[ind]:
-                ind += 1
-            indexes.append(ind)
-
-        px = px[:, indexes]
-        pw = np.zeros((1, NP)) + 1.0 / NP  # init weight
+    px = px[:, indexes]
+    pw = np.zeros((1, NP)) + 1.0 / NP  # init weight
 
     return px, pw
 

From 69772b4ad66cc27f7195659fff14e6c8006203b4 Mon Sep 17 00:00:00 2001
From: Mengge Jin <CH.Papaw@gmail.com>
Date: Sun, 27 Oct 2019 16:08:28 +0800
Subject: [PATCH 09/29] Remove PathLen in informed RRT* and instead with cBest

---
 PathPlanning/InformedRRTStar/informed_rrt_star.py | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py
index 4a32801d..6047660f 100644
--- a/PathPlanning/InformedRRTStar/informed_rrt_star.py
+++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py
@@ -40,7 +40,6 @@ class InformedRRTStar:
         self.node_list = [self.start]
         # max length we expect to find in our 'informed' sample space, starts as infinite
         cBest = float('inf')
-        pathLen = float('inf')
         solutionSet = set()
         path = None
 
@@ -89,7 +88,7 @@ class InformedRRTStar:
                     lastIndex = len(self.node_list) - 1
                     tempPath = self.get_final_course(lastIndex)
                     tempPathLen = self.get_path_len(tempPath)
-                    if tempPathLen < pathLen:
+                    if tempPathLen < cBest:
                         path = tempPath
                         cBest = tempPathLen
 

From 342e671b34f7cced14388b3b33fc4c4dd7dd119f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?=
 <20567087+goktug97@users.noreply.github.com>
Date: Fri, 25 Oct 2019 15:30:16 +0300
Subject: [PATCH 10/29] optimize nearest_neighbor_assosiation

- vectorize distance calculation
- append authors
---
 .../iterative_closest_point.py                | 24 +++++++------------
 1 file changed, 8 insertions(+), 16 deletions(-)

diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index 73651d8e..ec81bb12 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -1,6 +1,6 @@
 """
 Iterative Closest Point (ICP) SLAM example
-author: Atsushi Sakai (@Atsushi_twi)
+author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı
 """
 
 import math
@@ -39,7 +39,7 @@ def ICP_matching(ppoints, cpoints):
             plt.plot(cpoints[0, :], cpoints[1, :], ".b")
             plt.plot(0.0, 0.0, "xr")
             plt.axis("equal")
-            plt.pause(1.0)
+            plt.pause(0.1)
 
         inds, error = nearest_neighbor_assosiation(ppoints, cpoints)
         Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints)
@@ -93,18 +93,10 @@ def nearest_neighbor_assosiation(ppoints, cpoints):
     error = sum(d)
 
     # calc index with nearest neighbor assosiation
-    inds = []
-    for i in range(cpoints.shape[1]):
-        minid = -1
-        mind = float("inf")
-        for ii in range(ppoints.shape[1]):
-            d = np.linalg.norm(ppoints[:, ii] - cpoints[:, i])
-
-            if mind >= d:
-                mind = d
-                minid = ii
-
-        inds.append(minid)
+    d = np.linalg.norm(
+            np.repeat(cpoints, ppoints.shape[1], axis=1) - np.tile(ppoints, (1,
+                cpoints.shape[1])), axis=0)
+    inds = np.argmin(d.reshape(cpoints.shape[1], ppoints.shape[1]), axis=1)
 
     return inds, error
 
@@ -130,7 +122,7 @@ def main():
     print(__file__ + " start!!")
 
     # simulation parameters
-    nPoint = 10
+    nPoint = 1000
     fieldLength = 50.0
     motion = [0.5, 2.0, np.deg2rad(-10.0)]  # movement [x[m],y[m],yaw[deg]]
 
@@ -156,4 +148,4 @@ def main():
 
 
 if __name__ == '__main__':
-    main()
\ No newline at end of file
+    main()

From 97570de50c222c3362dfda765fd951f37cfb900f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?=
 <20567087+goktug97@users.noreply.github.com>
Date: Wed, 23 Oct 2019 10:14:14 +0300
Subject: [PATCH 11/29] Fix wrong rotation direction

- The obstacle rotation for the rectangle obstacle calculation
is wrong. It should be positive instead of negative.
---
 PathPlanning/DynamicWindowApproach/dynamic_window_approach.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
index c2a8674a..c8a44349 100644
--- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
+++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
@@ -2,7 +2,7 @@
 
 Mobile robot motion planning sample with Dynamic Window Approach
 
-author: Atsushi Sakai (@Atsushi_twi), Goktug Karakasli
+author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı
 
 """
 
@@ -172,7 +172,7 @@ def calc_obstacle_cost(trajectory, ob, config):
         rot = np.transpose(rot, [2, 0, 1])
         local_ob = ob[:, None] - trajectory[:, 0:2]
         local_ob = local_ob.reshape(-1, local_ob.shape[-1])
-        local_ob = np.array([local_ob @ -x for x in rot])
+        local_ob = np.array([local_ob @ x for x in rot])
         local_ob = local_ob.reshape(-1, local_ob.shape[-1])
         upper_check = local_ob[:, 0] <= config.robot_length / 2
         right_check = local_ob[:, 1] <= config.robot_width / 2

From 49ce57d6f8de1265eb1e578dd0f069c6b7a56ca6 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 27 Oct 2019 17:59:08 +0900
Subject: [PATCH 12/29] Code cleanup.

---
 .../iterative_closest_point.py                | 57 +++++++++----------
 tests/test_LQR_planner.py                     |  4 +-
 2 files changed, 30 insertions(+), 31 deletions(-)

diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index ec81bb12..3512ef97 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -4,22 +4,23 @@ author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı
 """
 
 import math
-import numpy as np
+
 import matplotlib.pyplot as plt
+import numpy as np
 
 #  ICP parameters
 EPS = 0.0001
-MAXITER = 100
+MAX_ITER = 100
 
 show_animation = True
 
 
-def ICP_matching(ppoints, cpoints):
+def icp_matching(previous_points, current_points):
     """
     Iterative Closest Point matching
     - input
-    ppoints: 2D points in the previous frame
-    cpoints: 2D points in the current frame
+    previous_points: 2D points in the previous frame
+    current_points: 2D points in the current frame
     - output
     R: Rotation matrix
     T: Translation vector
@@ -35,17 +36,17 @@ def ICP_matching(ppoints, cpoints):
 
         if show_animation:  # pragma: no cover
             plt.cla()
-            plt.plot(ppoints[0, :], ppoints[1, :], ".r")
-            plt.plot(cpoints[0, :], cpoints[1, :], ".b")
+            plt.plot(previous_points[0, :], previous_points[1, :], ".r")
+            plt.plot(current_points[0, :], current_points[1, :], ".b")
             plt.plot(0.0, 0.0, "xr")
             plt.axis("equal")
             plt.pause(0.1)
 
-        inds, error = nearest_neighbor_assosiation(ppoints, cpoints)
-        Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints)
+        indexes, error = nearest_neighbor_association(previous_points, current_points)
+        Rt, Tt = svd_motion_estimation(previous_points[:, indexes], current_points)
 
         # update current points
-        cpoints = (Rt @ cpoints) + Tt[:, np.newaxis]
+        current_points = (Rt @ current_points) + Tt[:, np.newaxis]
 
         H = update_homogeneous_matrix(H, Rt, Tt)
 
@@ -56,7 +57,7 @@ def ICP_matching(ppoints, cpoints):
         if dError <= EPS:
             print("Converge", error, dError, count)
             break
-        elif MAXITER <= count:
+        elif MAX_ITER <= count:
             print("Not Converge...", error, dError, count)
             break
 
@@ -85,31 +86,29 @@ def update_homogeneous_matrix(Hin, R, T):
         return Hin @ H
 
 
-def nearest_neighbor_assosiation(ppoints, cpoints):
+def nearest_neighbor_association(previous_points, current_points):
 
     # calc the sum of residual errors
-    dcpoints = ppoints - cpoints
-    d = np.linalg.norm(dcpoints, axis=0)
+    delta_points = previous_points - current_points
+    d = np.linalg.norm(delta_points, axis=0)
     error = sum(d)
 
     # calc index with nearest neighbor assosiation
-    d = np.linalg.norm(
-            np.repeat(cpoints, ppoints.shape[1], axis=1) - np.tile(ppoints, (1,
-                cpoints.shape[1])), axis=0)
-    inds = np.argmin(d.reshape(cpoints.shape[1], ppoints.shape[1]), axis=1)
+    d = np.linalg.norm(np.repeat(current_points, previous_points.shape[1], axis=1)
+                       - np.tile(previous_points, (1, current_points.shape[1])), axis=0)
+    indexes = np.argmin(d.reshape(current_points.shape[1], previous_points.shape[1]), axis=1)
 
-    return inds, error
+    return indexes, error
 
 
-def SVD_motion_estimation(ppoints, cpoints):
+def svd_motion_estimation(previous_points, current_points):
+    pm = np.mean(previous_points, axis=1)
+    cm = np.mean(current_points, axis=1)
 
-    pm = np.mean(ppoints, axis=1)
-    cm = np.mean(cpoints, axis=1)
+    p_shift = previous_points - pm[:, np.newaxis]
+    c_shift = current_points - cm[:, np.newaxis]
 
-    pshift = ppoints - pm[:, np.newaxis]
-    cshift = cpoints - cm[:, np.newaxis]
-
-    W = cshift @ pshift.T
+    W = c_shift @ p_shift.T
     u, s, vh = np.linalg.svd(W)
 
     R = (u @ vh).T
@@ -133,16 +132,16 @@ def main():
         # previous points
         px = (np.random.rand(nPoint) - 0.5) * fieldLength
         py = (np.random.rand(nPoint) - 0.5) * fieldLength
-        ppoints = np.vstack((px, py))
+        previous_points = np.vstack((px, py))
 
         # current points
         cx = [math.cos(motion[2]) * x - math.sin(motion[2]) * y + motion[0]
               for (x, y) in zip(px, py)]
         cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1]
               for (x, y) in zip(px, py)]
-        cpoints = np.vstack((cx, cy))
+        current_points = np.vstack((cx, cy))
 
-        R, T = ICP_matching(ppoints, cpoints)
+        R, T = icp_matching(previous_points, current_points)
         print("R:", R)
         print("T:", T)
 
diff --git a/tests/test_LQR_planner.py b/tests/test_LQR_planner.py
index 5ece27f8..2bcf828c 100644
--- a/tests/test_LQR_planner.py
+++ b/tests/test_LQR_planner.py
@@ -1,6 +1,6 @@
+import sys
 from unittest import TestCase
 
-import sys
 sys.path.append("./PathPlanning/LQRPlanner")
 
 from PathPlanning.LQRPlanner import LQRplanner as m
@@ -11,5 +11,5 @@ print(__file__)
 class Test(TestCase):
 
     def test1(self):
-        m.show_animation = False
+        m.SHOW_ANIMATION = False
         m.main()

From 3f14311cc54cb071b530a03b7a0245a61987c300 Mon Sep 17 00:00:00 2001
From: Francisco Moretti <franciscoemoretti@gmail.com>
Date: Sun, 3 Nov 2019 12:07:56 -0300
Subject: [PATCH 13/29] correct variable name

---
 PathPlanning/InformedRRTStar/informed_rrt_star.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py
index b9769bea..98a239ab 100644
--- a/PathPlanning/InformedRRTStar/informed_rrt_star.py
+++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py
@@ -74,9 +74,9 @@ class InformedRRTStar:
             newNode = self.get_new_node(theta, nind, nearestNode)
             d = self.line_cost(nearestNode, newNode)
 
-            isCollision = self.check_collision(nearestNode, theta, d)
+            noCollision = self.check_collision(nearestNode, theta, d)
 
-            if isCollision:
+            if noCollision:
                 nearInds = self.find_near_nodes(newNode)
                 newNode = self.choose_parent(newNode, nearInds)
 

From 178dca3e343749b9e3ec3d1fda7a89f464532cda Mon Sep 17 00:00:00 2001
From: Francisco Moretti <franciscoemoretti@gmail.com>
Date: Sun, 3 Nov 2019 12:12:53 -0300
Subject: [PATCH 14/29] add collision check to node near to goal

---
 .../InformedRRTStar/informed_rrt_star.py      | 21 ++++++++++++-------
 1 file changed, 13 insertions(+), 8 deletions(-)

diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py
index 98a239ab..a8352bc0 100644
--- a/PathPlanning/InformedRRTStar/informed_rrt_star.py
+++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py
@@ -84,6 +84,7 @@ class InformedRRTStar:
                 self.rewire(newNode, nearInds)
 
                 if self.is_near_goal(newNode):
+                    if self.check_segment_collision(newNode.x, newNode.y, self.goal.x , self.goal.y):
                     solutionSet.add(newNode)
                     lastIndex = len(self.node_list) - 1
                     tempPath = self.get_final_course(lastIndex)
@@ -240,18 +241,22 @@ class InformedRRTStar:
         projection = v + t * (w - v) # Projection falls on the segment
         return (p-projection).dot(p-projection)
 
+    def check_segment_collision(self, x1, y1, x2, y2):
+        for (ox, oy, size) in self.obstacle_list:
+            dd = self.distance_squared_point_to_segment(
+                np.array([x1, y1]),
+                np.array([x2, y2]),
+                np.array([ox, oy]))
+            if dd <= size**2:
+                return False  # collision
+        return True
+
+
     def check_collision(self, nearNode, theta, d):
         tmpNode = copy.deepcopy(nearNode)
         endx = tmpNode.x + math.cos(theta)*d
         endy = tmpNode.y + math.sin(theta)*d
-        for (ox, oy, size) in self.obstacle_list:
-            dd = self.distance_squared_point_to_segment(
-                np.array([tmpNode.x, tmpNode.y]),
-                np.array([endx, endy]),
-                np.array([ox, oy]))
-            if dd <= 1.1 * size**2:
-                return False  # collision
-        return True
+        return self.check_segment_collision(tmpNode.x, tmpNode.y, endx, endy)
 
     def get_final_course(self, lastIndex):
         path = [[self.goal.x, self.goal.y]]

From 3e5cad819247edd892ae7f8f0192170e8a4c31bd Mon Sep 17 00:00:00 2001
From: Francisco Moretti <franciscoemoretti@gmail.com>
Date: Sun, 3 Nov 2019 12:23:48 -0300
Subject: [PATCH 15/29] fix indentation

---
 PathPlanning/InformedRRTStar/informed_rrt_star.py | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py
index a8352bc0..62a94064 100644
--- a/PathPlanning/InformedRRTStar/informed_rrt_star.py
+++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py
@@ -85,13 +85,13 @@ class InformedRRTStar:
 
                 if self.is_near_goal(newNode):
                     if self.check_segment_collision(newNode.x, newNode.y, self.goal.x , self.goal.y):
-                    solutionSet.add(newNode)
-                    lastIndex = len(self.node_list) - 1
-                    tempPath = self.get_final_course(lastIndex)
-                    tempPathLen = self.get_path_len(tempPath)
-                    if tempPathLen < pathLen:
-                        path = tempPath
-                        cBest = tempPathLen
+                        solutionSet.add(newNode)
+                        lastIndex = len(self.node_list) - 1
+                        tempPath = self.get_final_course(lastIndex)
+                        tempPathLen = self.get_path_len(tempPath)
+                        if tempPathLen < pathLen:
+                            path = tempPath
+                            cBest = tempPathLen
 
             if animation:
                 self.draw_graph(xCenter=xCenter,

From 8b31c2bdf0cd1672f2282509e279540f76c2ef3a Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 17 Nov 2019 10:25:24 +0900
Subject: [PATCH 16/29] Create greetings.yml

---
 .github/workflows/greetings.yml | 13 +++++++++++++
 1 file changed, 13 insertions(+)
 create mode 100644 .github/workflows/greetings.yml

diff --git a/.github/workflows/greetings.yml b/.github/workflows/greetings.yml
new file mode 100644
index 00000000..3da4a694
--- /dev/null
+++ b/.github/workflows/greetings.yml
@@ -0,0 +1,13 @@
+name: Greetings
+
+on: [pull_request, issues]
+
+jobs:
+  greeting:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/first-interaction@v1
+      with:
+        repo-token: ${{ secrets.GITHUB_TOKEN }}
+        issue-message: 'Message that will be displayed on users'' This is first issue for you on this project'
+        pr-message: 'Message that will be displayed on users'' This is first pr for you on this project'

From 25ea5eb6c5a896350e6cd2c3ab2377ea2e9c1daa Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 17 Nov 2019 10:34:19 +0900
Subject: [PATCH 17/29] Create pythonpackage.yml

---
 .github/workflows/pythonpackage.yml | 33 +++++++++++++++++++++++++++++
 1 file changed, 33 insertions(+)
 create mode 100644 .github/workflows/pythonpackage.yml

diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml
new file mode 100644
index 00000000..901eb4c6
--- /dev/null
+++ b/.github/workflows/pythonpackage.yml
@@ -0,0 +1,33 @@
+name: Python package
+
+on: [push]
+
+jobs:
+  build:
+
+    runs-on: ubuntu-latest
+    strategy:
+      max-parallel: 4
+      matrix:
+        python-version: [3.5, 3.6, 3.7]
+
+    steps:
+    - uses: actions/checkout@v1
+    - name: Set up Python ${{ matrix.python-version }}
+      uses: actions/setup-python@v1
+      with:
+        python-version: ${{ matrix.python-version }}
+    - name: Install dependencies
+      run: |
+        python -m pip install --upgrade pip
+        pip install -r requirements.txt
+    - name: Lint with flake8
+      run: |
+        pip install flake8
+        # stop the build if there are Python syntax errors or undefined names
+        flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics
+        # 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: Test with pytest
+      run: |
+        runtests.sh

From 8877bc1a42d7853d949f00794385a16b488b3292 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 17 Nov 2019 10:41:28 +0900
Subject: [PATCH 18/29] Update pythonpackage.yml

---
 .github/workflows/pythonpackage.yml | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml
index 901eb4c6..c49c61de 100644
--- a/.github/workflows/pythonpackage.yml
+++ b/.github/workflows/pythonpackage.yml
@@ -28,6 +28,6 @@ jobs:
         flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics
         # 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: Test with pytest
-      run: |
-        runtests.sh
+    - name: Test
+      run: runtests.sh
+      shell: bash

From 4d9ff51f27a11dbf615b5755781a3e1e961bb5a8 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 17 Nov 2019 12:00:00 +0900
Subject: [PATCH 19/29] Update pythonpackage.yml

---
 .github/workflows/pythonpackage.yml | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml
index c49c61de..b9f0adb8 100644
--- a/.github/workflows/pythonpackage.yml
+++ b/.github/workflows/pythonpackage.yml
@@ -28,6 +28,9 @@ jobs:
         flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics
         # 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: Test1
+      run: ls
+      shell: bash
     - name: Test
       run: runtests.sh
       shell: bash

From ecdff00fa2ee0d870b0c4b01fd2c92c52389ce21 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 17 Nov 2019 13:15:57 +0900
Subject: [PATCH 20/29] Update pythonpackage.yml

---
 .github/workflows/pythonpackage.yml | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml
index b9f0adb8..4d6e3d3b 100644
--- a/.github/workflows/pythonpackage.yml
+++ b/.github/workflows/pythonpackage.yml
@@ -28,9 +28,5 @@ jobs:
         flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics
         # 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: Test1
-      run: ls
-      shell: bash
     - name: Test
-      run: runtests.sh
-      shell: bash
+      run: bash runtests.sh

From 27b6e19ea6f9d4b4e08b23b5db305e79a624e6bf Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 17 Nov 2019 13:21:14 +0900
Subject: [PATCH 21/29] Update pythonpackage.yml

---
 .github/workflows/pythonpackage.yml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml
index 4d6e3d3b..fbac68ac 100644
--- a/.github/workflows/pythonpackage.yml
+++ b/.github/workflows/pythonpackage.yml
@@ -28,5 +28,7 @@ jobs:
         flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics
         # 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 insall coverage   
     - name: Test
       run: bash runtests.sh

From 157664857b9eb12181e1405c28b18928d7120a59 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 17 Nov 2019 13:23:30 +0900
Subject: [PATCH 22/29] Update pythonpackage.yml

---
 .github/workflows/pythonpackage.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml
index fbac68ac..fc708457 100644
--- a/.github/workflows/pythonpackage.yml
+++ b/.github/workflows/pythonpackage.yml
@@ -29,6 +29,6 @@ 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 insall coverage   
+      run: pip install coverage   
     - name: Test
       run: bash runtests.sh

From 3333b34ce215b0287073fd250aa0c2813ed36e48 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 17 Nov 2019 13:33:50 +0900
Subject: [PATCH 23/29] Update pythonpackage.yml

---
 .github/workflows/pythonpackage.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml
index fc708457..a3203ff1 100644
--- a/.github/workflows/pythonpackage.yml
+++ b/.github/workflows/pythonpackage.yml
@@ -9,7 +9,7 @@ jobs:
     strategy:
       max-parallel: 4
       matrix:
-        python-version: [3.5, 3.6, 3.7]
+        python-version: [3.6, 3.7]
 
     steps:
     - uses: actions/checkout@v1

From 0067577cbe500054bafc5ce6dd00a3effc68f88e Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 17 Nov 2019 14:33:24 +0900
Subject: [PATCH 24/29] remove PoseOptimizationSLAM

---
 README.md                                     | 17 -----------
 SLAM/PoseOptimizationSLAM2D/README.md         |  9 ------
 .../PoseOptimizationSLAM2D/data_downloader.py | 28 -------------------
 SLAM/PoseOptimizationSLAM3D/README.md         | 25 -----------------
 .../PoseOptimizationSLAM3D/data_downloader.py | 26 -----------------
 5 files changed, 105 deletions(-)
 delete mode 100644 SLAM/PoseOptimizationSLAM2D/README.md
 delete mode 100644 SLAM/PoseOptimizationSLAM2D/data_downloader.py
 delete mode 100644 SLAM/PoseOptimizationSLAM3D/README.md
 delete mode 100644 SLAM/PoseOptimizationSLAM3D/data_downloader.py

diff --git a/README.md b/README.md
index 1c455e92..e740049b 100644
--- a/README.md
+++ b/README.md
@@ -31,8 +31,6 @@ Python codes for robotics algorithm.
    * [SLAM](#slam)
       * [Iterative Closest Point (ICP) Matching](#iterative-closest-point-icp-matching)
       * [FastSLAM 1.0](#fastslam-10)
-      * [Pose Optimization SLAM 2D](#pose-optimization-slam-2d)
-      * [Pose Optimization SLAM 3D](#pose-optimization-slam-3d)
    * [Path Planning](#path-planning)
       * [Dynamic Window Approach](#dynamic-window-approach)
       * [Grid based search](#grid-based-search)
@@ -251,21 +249,6 @@ Ref:
 - [SLAM simulations by Tim Bailey](http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm)
 
 
-## Pose Optimization SLAM 2D
-
-This is a graph based 2D pose optimization SLAM example.
-
-![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/PoseOptimizationSLAM2D/animation.gif)
-
-
-## Pose Optimization SLAM 3D
-
-This is a graph based 3D pose optimization SLAM example.
-
-![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/PoseOptimizationSLAM3D/animation.gif)
-
-
-
 # Path Planning
 
 ## Dynamic Window Approach
diff --git a/SLAM/PoseOptimizationSLAM2D/README.md b/SLAM/PoseOptimizationSLAM2D/README.md
deleted file mode 100644
index 612fa518..00000000
--- a/SLAM/PoseOptimizationSLAM2D/README.md
+++ /dev/null
@@ -1,9 +0,0 @@
-# How to use
-
-1. Download data
-
->$ python data_downloader.py
-
-2. run SLAM
-
->$ python pose_optimization_slam.py
diff --git a/SLAM/PoseOptimizationSLAM2D/data_downloader.py b/SLAM/PoseOptimizationSLAM2D/data_downloader.py
deleted file mode 100644
index d1b8a2c2..00000000
--- a/SLAM/PoseOptimizationSLAM2D/data_downloader.py
+++ /dev/null
@@ -1,28 +0,0 @@
-"""
-
-Data down loader for pose optimization
-
-author: Atsushi Sakai
-
-"""
-
-
-import subprocess
-def main():
-    print("start!!")
-
-    cmd = "wget https://www.dropbox.com/s/vcz8cag7bo0zlaj/input_INTEL_g2o.g2o?dl=0 -O intel.g2o -nc"
-    subprocess.call(cmd, shell=True)
-
-    cmd = "wget https://www.dropbox.com/s/d8fcn1jg1mebx8f/input_MITb_g2o.g2o?dl=0 -O mit_killian.g2o -nc"
-
-    subprocess.call(cmd, shell=True)
-    cmd = "wget https://www.dropbox.com/s/gmdzo74b3tzvbrw/input_M3500_g2o.g2o?dl=0 -O manhattan3500.g2o -nc"
-    subprocess.call(cmd, shell=True)
-
-    print("done!!")
-
-
-if __name__ == '__main__':
-    main()
-
diff --git a/SLAM/PoseOptimizationSLAM3D/README.md b/SLAM/PoseOptimizationSLAM3D/README.md
deleted file mode 100644
index 396a6297..00000000
--- a/SLAM/PoseOptimizationSLAM3D/README.md
+++ /dev/null
@@ -1,25 +0,0 @@
-# PoseOptimizationSLAM3D
-3D (x, y, z, qw, qx, qy, qz) pose optimization SLAM
-
-## How to use 
-1. Download data  
-
-~~~
-python data_downloader.py
-~~~
-
-2. run SLAM 
-
-~~~
-python pose_optimization_slam_3d.py
-~~~
-
-## Reference 
-- [A Compact and Portable Implementation of Graph\-based SLAM](https://www.researchgate.net/publication/321287640_A_Compact_and_Portable_Implementation_of_Graph-based_SLAM)  
-
-- [GitHub \- furo\-org/p2o: Single header 2D/3D graph\-based SLAM library](https://github.com/furo-org/p2o)  
-
-- [GitHub \- AtsushiSakai/PythonRobotics
-/SLAM/PoseOptimizationSLAM](https://github.com/AtsushiSakai/PythonRobotics/tree/master/SLAM/PoseOptimizationSLAM)
-
-- [リー代数による3次元ポーズ調整\(Pose Adjustment\)\[PythonRobotics\]\[SLAM\] \- Qiita](https://qiita.com/saitosasaki/items/a0540cba994f08bf5e16#comment-3e6588e6b096cc2567d8)
diff --git a/SLAM/PoseOptimizationSLAM3D/data_downloader.py b/SLAM/PoseOptimizationSLAM3D/data_downloader.py
deleted file mode 100644
index a0c04fd1..00000000
--- a/SLAM/PoseOptimizationSLAM3D/data_downloader.py
+++ /dev/null
@@ -1,26 +0,0 @@
-"""
-
-Data down loader for pose optimization
-
-author: Atsushi Sakai
-
-"""
-
-import subprocess
-
-
-def main():
-    print("start!!")
-
-    cmd = "wget https://www.dropbox.com/s/zu23p8d522qccor/parking-garage.g2o?dl=0 -O parking-garage.g2o -nc"
-    subprocess.call(cmd, shell=True)
-    cmd = "wget http://www.furo.org/irie/datasets/torus3d_guess.g2o -nc"
-    subprocess.call(cmd, shell=True)
-    cmd = "wget http://www.furo.org/irie/datasets/sphere2200_guess.g2o -nc"
-    subprocess.call(cmd, shell=True)
-
-    print("done!!")
-
-
-if __name__ == '__main__':
-    main()

From 54991efb8f335faf42fa665dce9dfecdc2833810 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Mon, 25 Nov 2019 21:54:32 +0900
Subject: [PATCH 25/29] Update FUNDING.yml

---
 .github/FUNDING.yml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.github/FUNDING.yml b/.github/FUNDING.yml
index 80f4984c..24f09262 100644
--- a/.github/FUNDING.yml
+++ b/.github/FUNDING.yml
@@ -1,3 +1,4 @@
 # These are supported funding model platforms
+github:  AtsushiSakai
 patreon: myenigma
 custom: https://www.paypal.me/myenigmapay/

From 2fe00f8824842452345cbe180b4303e240c18a6c Mon Sep 17 00:00:00 2001
From: Michael Dobler <michael.dobler@outlook.com>
Date: Thu, 28 Nov 2019 23:35:55 +0100
Subject: [PATCH 26/29] Fix jacobian of observation

---
 SLAM/EKFSLAM/ekf_slam.ipynb | 2 +-
 SLAM/EKFSLAM/ekf_slam.py    | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/SLAM/EKFSLAM/ekf_slam.ipynb b/SLAM/EKFSLAM/ekf_slam.ipynb
index 6509373d..f8009395 100644
--- a/SLAM/EKFSLAM/ekf_slam.ipynb
+++ b/SLAM/EKFSLAM/ekf_slam.ipynb
@@ -479,7 +479,7 @@
     "    \"\"\"\n",
     "    sq = math.sqrt(q)\n",
     "    G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],\n",
-    "                  [delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])\n",
+    "                  [delta[1, 0], - delta[0, 0], - q, - delta[1, 0], delta[0, 0]]])\n",
     "\n",
     "    G = G / q\n",
     "    nLM = calc_n_LM(x)\n",
diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py
index ae39c9e0..84cf4d97 100644
--- a/SLAM/EKFSLAM/ekf_slam.py
+++ b/SLAM/EKFSLAM/ekf_slam.py
@@ -177,7 +177,7 @@ def calc_innovation(lm, xEst, PEst, z, LMid):
 def jacob_h(q, delta, x, i):
     sq = math.sqrt(q)
     G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
-                  [delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])
+                  [delta[1, 0], - delta[0, 0], - q, - delta[1, 0], delta[0, 0]]])
 
     G = G / q
     nLM = calc_n_lm(x)

From 0b07425d2e8d97f61fb456d0969e1a64eeee33ba Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sat, 30 Nov 2019 20:05:52 +0900
Subject: [PATCH 27/29] Code clen up

---
 SLAM/EKFSLAM/ekf_slam.py | 33 ++++++++++++++++-----------------
 1 file changed, 16 insertions(+), 17 deletions(-)

diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py
index 84cf4d97..27e50354 100644
--- a/SLAM/EKFSLAM/ekf_slam.py
+++ b/SLAM/EKFSLAM/ekf_slam.py
@@ -1,5 +1,6 @@
 """
 Extended Kalman Filter SLAM example
+
 author: Atsushi Sakai (@Atsushi_twi)
 """
 
@@ -35,10 +36,10 @@ def ekf_slam(xEst, PEst, u, z):
 
     # Update
     for iz in range(len(z[:, 0])):  # for each observation
-        minid = search_correspond_landmark_id(xEst, PEst, z[iz, 0:2])
+        min_id = search_correspond_landmark_id(xEst, PEst, z[iz, 0:2])
 
         nLM = calc_n_lm(xEst)
-        if minid == nLM:
+        if min_id == nLM:
             print("New LM")
             # Extend state and covariance matrix
             xAug = np.vstack((xEst, calc_landmark_position(xEst, z[iz, :])))
@@ -46,8 +47,8 @@ def ekf_slam(xEst, PEst, u, z):
                               np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))
             xEst = xAug
             PEst = PAug
-        lm = get_landmark_position_from_state(xEst, minid)
-        y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)
+        lm = get_landmark_position_from_state(xEst, min_id)
+        y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], min_id)
 
         K = (PEst @ H.T) @ np.linalg.inv(S)
         xEst = xEst + (K @ y)
@@ -60,8 +61,8 @@ def ekf_slam(xEst, PEst, u, z):
 
 def calc_input():
     v = 1.0  # [m/s]
-    yawrate = 0.1  # [rad/s]
-    u = np.array([[v, yawrate]]).T
+    yaw_rate = 0.1  # [rad/s]
+    u = np.array([[v, yaw_rate]]).T
     return u
 
 
@@ -79,8 +80,8 @@ def observation(xTrue, xd, u, RFID):
         angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
         if d <= MAX_RANGE:
             dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5  # add noise
-            anglen = angle + np.random.randn() * Q_sim[1, 1] ** 0.5  # add noise
-            zi = np.array([dn, anglen, i])
+            angle_n = angle + np.random.randn() * Q_sim[1, 1] ** 0.5  # add noise
+            zi = np.array([dn, angle_n, i])
             z = np.vstack((z, zi))
 
     # add noise to input
@@ -128,8 +129,6 @@ def calc_landmark_position(x, z):
 
     zp[0, 0] = x[0, 0] + z[0] * math.cos(x[2, 0] + z[1])
     zp[1, 0] = x[1, 0] + z[0] * math.sin(x[2, 0] + z[1])
-    # zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1])
-    # zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1])
 
     return zp
 
@@ -147,25 +146,25 @@ def search_correspond_landmark_id(xAug, PAug, zi):
 
     nLM = calc_n_lm(xAug)
 
-    mdist = []
+    min_dist = []
 
     for i in range(nLM):
         lm = get_landmark_position_from_state(xAug, i)
         y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
-        mdist.append(y.T @ np.linalg.inv(S) @ y)
+        min_dist.append(y.T @ np.linalg.inv(S) @ y)
 
-    mdist.append(M_DIST_TH)  # new landmark
+    min_dist.append(M_DIST_TH)  # new landmark
 
-    minid = mdist.index(min(mdist))
+    min_id = min_dist.index(min(min_dist))
 
-    return minid
+    return min_id
 
 
 def calc_innovation(lm, xEst, PEst, z, LMid):
     delta = lm - xEst[0:2]
     q = (delta.T @ delta)[0, 0]
-    zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0]
-    zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]])
+    z_angle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0]
+    zp = np.array([[math.sqrt(q), pi_2_pi(z_angle)]])
     y = (z - zp).T
     y[1] = pi_2_pi(y[1])
     H = jacob_h(q, delta, xEst, LMid + 1)

From f26a91d5342c974b78e3cc3c820518c3400a08f7 Mon Sep 17 00:00:00 2001
From: harderthan <kheo1772@gmail.com>
Date: Tue, 3 Dec 2019 12:46:58 +0900
Subject: [PATCH 28/29] Update Kalmanfilter_basics.rst

Edited the misspelling
---
 docs/modules/Kalmanfilter_basics.rst | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/modules/Kalmanfilter_basics.rst b/docs/modules/Kalmanfilter_basics.rst
index ce84fe0d..7325dafd 100644
--- a/docs/modules/Kalmanfilter_basics.rst
+++ b/docs/modules/Kalmanfilter_basics.rst
@@ -180,7 +180,7 @@ Central Limit Theorem
 ^^^^^^^^^^^^^^^^^^^^^
 
 According to this theorem, the average of n samples of random and
-independant variables tends to follow a normal distribution as we
+independent variables tends to follow a normal distribution as we
 increase the sample size.(Generally, for n>=30)
 
 .. code-block:: ipython3

From 338b7945b6e12511dc1ae19166c1f32c3e9d313e Mon Sep 17 00:00:00 2001
From: harderthan <kheo1772@gmail.com>
Date: Tue, 3 Dec 2019 12:49:52 +0900
Subject: [PATCH 29/29] Update Kalmanfilter_basics

Edited misspelling
---
 Localization/Kalmanfilter_basics.ipynb | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Localization/Kalmanfilter_basics.ipynb b/Localization/Kalmanfilter_basics.ipynb
index 37428791..b4913f9e 100644
--- a/Localization/Kalmanfilter_basics.ipynb
+++ b/Localization/Kalmanfilter_basics.ipynb
@@ -228,7 +228,7 @@
     "\n",
     "#### Central Limit Theorem\n",
     "\n",
-    "According to this theorem, the average of n samples of random and independant variables tends to follow a normal distribution as we increase the sample size.(Generally, for n>=30)"
+    "According to this theorem, the average of n samples of random and independent variables tends to follow a normal distribution as we increase the sample size.(Generally, for n>=30)"
    ]
   },
   {