Merge pull request #1 from AtsushiSakai/master

Update

.gitignore

@@ -4,6 +4,8 @@
 __pycache__/
 *.py[cod]
 *$py.class
+_build/
+.idea/
 
 # C extensions
 *.so

.gitmodules

@@ -0,0 +1,4 @@
+[submodule "doc/PythonRoboticsPaper"]
+	path = doc/PythonRoboticsPaper
+	url = https://github.com/AtsushiSakai/PythonRoboticsPaper
+

.travis.yml

@@ -3,6 +3,7 @@ python:
   - 3.6
 
 before_install:
+    - sudo apt-get update
     - wget https://repo.continuum.io/miniconda/Miniconda3-latest-Linux-x86_64.sh -O miniconda.sh
     - chmod +x miniconda.sh
     - bash miniconda.sh -b -p $HOME/miniconda
@@ -14,13 +15,15 @@ before_install:
     - conda info -a
 
 install:
-  - pip install scipy
-  - pip install numpy
-  - pip install matplotlib
-  - pip install pandas
-  - pip install cvxpy
-  - conda install -y -c cvxgrp cvxpy
-  - conda install -y -c anaconda cvxopt
+  - conda install numpy
+  - conda install scipy
+  - conda install matplotlib
+  - conda install pandas
+  - conda install -c conda-forge lapack
+  - conda install -c cvxgrp cvxpy
+  #- pip install cvxpy
+  #- conda install -y -c cvxgrp cvxpy
+  #- conda install -y -c anaconda cvxopt
 
 script:
   - python --version

AerialNavigation/drone_3d_trajectory_following/Quadrotor.py

@@ -9,24 +9,26 @@ import numpy as np
 import matplotlib.pyplot as plt
 from mpl_toolkits.mplot3d import Axes3D
 
+
 class Quadrotor():
-    def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25):
-        self.p1 = np.array([size/2, 0, 0, 1]).T
-        self.p2 = np.array([-size/2, 0, 0, 1]).T
-        self.p3 = np.array([0, size/2, 0, 1]).T
-        self.p4 = np.array([0, -size/2, 0, 1]).T
+    def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25, show_animation=True):
+        self.p1 = np.array([size / 2, 0, 0, 1]).T
+        self.p2 = np.array([-size / 2, 0, 0, 1]).T
+        self.p3 = np.array([0, size / 2, 0, 1]).T
+        self.p4 = np.array([0, -size / 2, 0, 1]).T
 
         self.x_data = []
         self.y_data = []
         self.z_data = []
-
-        plt.ion()
-
-        fig = plt.figure()
-        self.ax = fig.add_subplot(111, projection='3d')
+        self.show_animation = show_animation
 
         self.update_pose(x, y, z, roll, pitch, yaw)
 
+        if self.show_animation:
+            plt.ion()
+            fig = plt.figure()
+            self.ax = fig.add_subplot(111, projection='3d')
+
     def update_pose(self, x, y, z, roll, pitch, yaw):
         self.x = x
         self.y = y
@@ -37,7 +39,9 @@ class Quadrotor():
         self.x_data.append(x)
         self.y_data.append(y)
         self.z_data.append(z)
-        self.plot()
+
+        if self.show_animation:
+            self.plot()
 
     def transformation_matrix(self):
         x = self.x
@@ -47,10 +51,11 @@ class Quadrotor():
         pitch = self.pitch
         yaw = self.yaw
         return np.array(
-            [[cos(yaw)*cos(pitch), -sin(yaw)*cos(roll)+cos(yaw)*sin(pitch)*sin(roll), sin(yaw)*sin(roll)+cos(yaw)*sin(pitch)*cos(roll), x],
-             [sin(yaw)*cos(pitch), cos(yaw)*cos(roll)+sin(yaw)*sin(pitch)*sin(roll), -cos(yaw)*sin(roll)+sin(yaw)*sin(pitch)*cos(roll), y],
-             [-sin(pitch), cos(pitch)*sin(roll), cos(pitch)*cos(yaw), z]
-            ])
+            [[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x],
+             [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) *
+              sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
+             [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z]
+             ])
 
     def plot(self):
         T = self.transformation_matrix()
@@ -66,8 +71,10 @@ class Quadrotor():
                      [p1_t[1], p2_t[1], p3_t[1], p4_t[1]],
                      [p1_t[2], p2_t[2], p3_t[2], p4_t[2]], 'k.')
 
-        self.ax.plot([p1_t[0], p2_t[0]], [p1_t[1], p2_t[1]], [p1_t[2], p2_t[2]], 'r-')
-        self.ax.plot([p3_t[0], p4_t[0]], [p3_t[1], p4_t[1]], [p3_t[2], p4_t[2]], 'r-')
+        self.ax.plot([p1_t[0], p2_t[0]], [p1_t[1], p2_t[1]],
+                     [p1_t[2], p2_t[2]], 'r-')
+        self.ax.plot([p3_t[0], p4_t[0]], [p3_t[1], p4_t[1]],
+                     [p3_t[2], p4_t[2]], 'r-')
 
         self.ax.plot(self.x_data, self.y_data, self.z_data, 'b:')
 
@@ -75,5 +82,4 @@ class Quadrotor():
         plt.ylim(-5, 5)
         self.ax.set_zlim(0, 10)
 
-        plt.show()
         plt.pause(0.001)

AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py

@@ -9,7 +9,9 @@ import numpy as np
 from Quadrotor import Quadrotor
 from TrajectoryGenerator import TrajectoryGenerator
 
-#Simulation parameters
+show_animation = True
+
+# Simulation parameters
 g = 9.81
 m = 0.2
 Ixx = 1
@@ -17,7 +19,7 @@ Iyy = 1
 Izz = 1
 T = 5
 
-#Proportional coefficients
+# Proportional coefficients
 Kp_x = 1
 Kp_y = 1
 Kp_z = 1
@@ -25,11 +27,12 @@ Kp_roll = 25
 Kp_pitch = 25
 Kp_yaw = 25
 
-#Derivative coefficients
+# Derivative coefficients
 Kd_x = 10
 Kd_y = 10
 Kd_z = 1
 
+
 def quad_sim(x_c, y_c, z_c):
     """
     Calculates the necessary thrust and torques for the quadrotor to
@@ -57,9 +60,12 @@ def quad_sim(x_c, y_c, z_c):
     dt = 0.1
     t = 0
 
-    q = Quadrotor(x=x_pos, y=y_pos, z=z_pos, roll=roll, pitch=pitch, yaw=yaw, size=1)
+    q = Quadrotor(x=x_pos, y=y_pos, z=z_pos, roll=roll,
+                  pitch=pitch, yaw=yaw, size=1, show_animation=show_animation)
 
     i = 0
+    n_run = 8
+    irun = 0
 
     while True:
         while t <= T:
@@ -73,38 +79,48 @@ def quad_sim(x_c, y_c, z_c):
             des_y_acc = calculate_acceleration(y_c[i], t)
             des_z_acc = calculate_acceleration(z_c[i], t)
 
-            thrust = m*(g + des_z_acc + Kp_z*(des_z_pos - z_pos) + Kd_z*(des_z_vel - z_vel))
+            thrust = m * (g + des_z_acc + Kp_z * (des_z_pos -
+                                                  z_pos) + Kd_z * (des_z_vel - z_vel))
 
-            roll_torque = Kp_roll*(((des_x_acc*sin(des_yaw) - des_y_acc*cos(des_yaw))/g) - roll)
-            pitch_torque = Kp_pitch*(((des_x_acc*cos(des_yaw) - des_y_acc*sin(des_yaw))/g) - pitch)
-            yaw_torque = Kp_yaw*(des_yaw - yaw)
+            roll_torque = Kp_roll * \
+                (((des_x_acc * sin(des_yaw) - des_y_acc * cos(des_yaw)) / g) - roll)
+            pitch_torque = Kp_pitch * \
+                (((des_x_acc * cos(des_yaw) - des_y_acc * sin(des_yaw)) / g) - pitch)
+            yaw_torque = Kp_yaw * (des_yaw - yaw)
 
-            roll_vel += roll_torque*dt/Ixx
-            pitch_vel += pitch_torque*dt/Iyy
-            yaw_vel += yaw_torque*dt/Izz
+            roll_vel += roll_torque * dt / Ixx
+            pitch_vel += pitch_torque * dt / Iyy
+            yaw_vel += yaw_torque * dt / Izz
 
-            roll += roll_vel*dt
-            pitch += pitch_vel*dt
-            yaw += yaw_vel*dt
+            roll += roll_vel * dt
+            pitch += pitch_vel * dt
+            yaw += yaw_vel * dt
 
             R = rotation_matrix(roll, pitch, yaw)
-            acc = (np.matmul(R, np.array([0, 0, thrust]).T) - np.array([0, 0, m*g]).T)/m
+            acc = (np.matmul(R, np.array(
+                [0, 0, thrust]).T) - np.array([0, 0, m * g]).T) / m
             x_acc = acc[0]
             y_acc = acc[1]
             z_acc = acc[2]
-            x_vel += x_acc*dt
-            y_vel += y_acc*dt
-            z_vel += z_acc*dt
-            x_pos += x_vel*dt
-            y_pos += y_vel*dt
-            z_pos += z_vel*dt
+            x_vel += x_acc * dt
+            y_vel += y_acc * dt
+            z_vel += z_acc * dt
+            x_pos += x_vel * dt
+            y_pos += y_vel * dt
+            z_pos += z_vel * dt
 
             q.update_pose(x_pos, y_pos, z_pos, roll, pitch, yaw)
 
             t += dt
 
         t = 0
-        i = (i+1)%4
+        i = (i + 1) % 4
+        irun += 1
+        if irun >= n_run:
+            break
+
+    print("Done")
+
 
 def calculate_position(c, t):
     """
@@ -118,7 +134,8 @@ def calculate_position(c, t):
     Returns
         Position
     """
-    return c[0]*t**5 + c[1]*t**4 + c[2]*t**3 + c[3]*t**2 + c[4]*t + c[5]
+    return c[0] * t**5 + c[1] * t**4 + c[2] * t**3 + c[3] * t**2 + c[4] * t + c[5]
+
 
 def calculate_velocity(c, t):
     """
@@ -132,7 +149,8 @@ def calculate_velocity(c, t):
     Returns
         Velocity
     """
-    return 5*c[0]*t**4 + 4*c[1]*t**3 + 3*c[2]*t**2 + 2*c[3]*t + c[4]
+    return 5 * c[0] * t**4 + 4 * c[1] * t**3 + 3 * c[2] * t**2 + 2 * c[3] * t + c[4]
+
 
 def calculate_acceleration(c, t):
     """
@@ -146,7 +164,8 @@ def calculate_acceleration(c, t):
     Returns
         Acceleration
     """
-    return 20*c[0]*t**3 + 12*c[1]*t**2 + 6*c[2]*t + 2*c[3]
+    return 20 * c[0] * t**3 + 12 * c[1] * t**2 + 6 * c[2] * t + 2 * c[3]
+
 
 def rotation_matrix(roll, pitch, yaw):
     """
@@ -161,10 +180,12 @@ def rotation_matrix(roll, pitch, yaw):
         3x3 rotation matrix as NumPy array
     """
     return np.array(
-        [[cos(yaw)*cos(pitch), -sin(yaw)*cos(roll)+cos(yaw)*sin(pitch)*sin(roll), sin(yaw)*sin(roll)+cos(yaw)*sin(pitch)*cos(roll)],
-         [sin(yaw)*cos(pitch), cos(yaw)*cos(roll)+sin(yaw)*sin(pitch)*sin(roll), -cos(yaw)*sin(roll)+sin(yaw)*sin(pitch)*cos(roll)],
-         [-sin(pitch), cos(pitch)*sin(roll), cos(pitch)*cos(yaw)]
-        ])
+        [[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll)],
+         [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) *
+          sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll)],
+         [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw)]
+         ])
+
 
 def main():
     """
@@ -177,7 +198,7 @@ def main():
     waypoints = [[-5, -5, 5], [5, -5, 5], [5, 5, 5], [-5, 5, 5]]
 
     for i in range(4):
-        traj = TrajectoryGenerator(waypoints[i], waypoints[(i+1)%4], T)
+        traj = TrajectoryGenerator(waypoints[i], waypoints[(i + 1) % 4], T)
         traj.solve()
         x_coeffs[i] = traj.x_c
         y_coeffs[i] = traj.y_c
@@ -185,5 +206,6 @@ def main():
 
     quad_sim(x_coeffs, y_coeffs, z_coeffs)
 
+
 if __name__ == "__main__":
     main()

ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py

@@ -7,50 +7,61 @@ Author: Daniel Ingram
 import numpy as np
 import matplotlib.pyplot as plt
 
+
 class NLinkArm(object):
-    def __init__(self, link_lengths, joint_angles, goal):
+    def __init__(self, link_lengths, joint_angles, goal, show_animation):
+        self.show_animation = show_animation
         self.n_links = len(link_lengths)
         if self.n_links is not len(joint_angles):
             raise ValueError()
 
         self.link_lengths = np.array(link_lengths)
         self.joint_angles = np.array(joint_angles)
-        self.points = [[0, 0] for _ in range(self.n_links+1)]
+        self.points = [[0, 0] for _ in range(self.n_links + 1)]
 
         self.lim = sum(link_lengths)
         self.goal = np.array(goal).T
 
-        self.fig = plt.figure()
-        self.fig.canvas.mpl_connect('button_press_event', self.click)
+        if show_animation:
+            self.fig = plt.figure()
+            self.fig.canvas.mpl_connect('button_press_event', self.click)
 
-        plt.ion()
-        plt.show()
+            plt.ion()
+            plt.show()
 
         self.update_points()
 
     def update_joints(self, joint_angles):
         self.joint_angles = joint_angles
+
         self.update_points()
 
     def update_points(self):
-        for i in range(1, self.n_links+1):
-            self.points[i][0] = self.points[i-1][0] + self.link_lengths[i-1]*np.cos(np.sum(self.joint_angles[:i]))
-            self.points[i][1] = self.points[i-1][1] + self.link_lengths[i-1]*np.sin(np.sum(self.joint_angles[:i]))
+        for i in range(1, self.n_links + 1):
+            self.points[i][0] = self.points[i - 1][0] + \
+                self.link_lengths[i - 1] * \
+                np.cos(np.sum(self.joint_angles[:i]))
+            self.points[i][1] = self.points[i - 1][1] + \
+                self.link_lengths[i - 1] * \
+                np.sin(np.sum(self.joint_angles[:i]))
 
         self.end_effector = np.array(self.points[self.n_links]).T
-        self.plot()
+        if self.show_animation:
+            self.plot()
 
     def plot(self):
         plt.cla()
 
-        for i in range(self.n_links+1):
+        for i in range(self.n_links + 1):
             if i is not self.n_links:
-                plt.plot([self.points[i][0], self.points[i+1][0]], [self.points[i][1], self.points[i+1][1]], 'r-')
+                plt.plot([self.points[i][0], self.points[i + 1][0]],
+                         [self.points[i][1], self.points[i + 1][1]], 'r-')
             plt.plot(self.points[i][0], self.points[i][1], 'ko')
 
         plt.plot(self.goal[0], self.goal[1], 'gx')
 
-        plt.plot([self.end_effector[0], self.goal[0]], [self.end_effector[1], self.goal[1]], 'g--')
+        plt.plot([self.end_effector[0], self.goal[0]], [
+                 self.end_effector[1], self.goal[1]], 'g--')
 
         plt.xlim([-self.lim, self.lim])
         plt.ylim([-self.lim, self.lim])

ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py

@@ -2,31 +2,35 @@
 Inverse kinematics for an n-link arm using the Jacobian inverse method
 
 Author: Daniel Ingram (daniel-s-ingram)
+        Atsushi Sakai (@Atsushi_twi)
 """
 import matplotlib.pyplot as plt
 import numpy as np
 
 from NLinkArm import NLinkArm
 
-#Simulation parameters
+# Simulation parameters
 Kp = 2
 dt = 0.1
 N_LINKS = 10
 N_ITERATIONS = 10000
 
-#States
+# States
 WAIT_FOR_NEW_GOAL = 1
 MOVING_TO_GOAL = 2
 
-def n_link_arm_ik():
+show_animation = True
+
+
+def main():
     """
     Creates an arm using the NLinkArm class and uses its inverse kinematics
     to move it to the desired position.
     """
-    link_lengths = [1]*N_LINKS
-    joint_angles = np.array([0]*N_LINKS)
+    link_lengths = [1] * N_LINKS
+    joint_angles = np.array([0] * N_LINKS)
     goal_pos = [N_LINKS, 0]
-    arm = NLinkArm(link_lengths, joint_angles, goal_pos)
+    arm = NLinkArm(link_lengths, joint_angles, goal_pos, show_animation)
     state = WAIT_FOR_NEW_GOAL
     solution_found = False
     while True:
@@ -35,10 +39,11 @@ def n_link_arm_ik():
         end_effector = arm.end_effector
         errors, distance = distance_to_goal(end_effector, goal_pos)
 
-        #State machine to allow changing of goal before current goal has been reached
+        # State machine to allow changing of goal before current goal has been reached
         if state is WAIT_FOR_NEW_GOAL:
             if distance > 0.1 and not solution_found:
-                joint_goal_angles, solution_found = inverse_kinematics(link_lengths, joint_angles, goal_pos)
+                joint_goal_angles, solution_found = inverse_kinematics(
+                    link_lengths, joint_angles, goal_pos)
                 if not solution_found:
                     print("Solution could not be found.")
                     state = WAIT_FOR_NEW_GOAL
@@ -47,13 +52,14 @@ def n_link_arm_ik():
                     state = MOVING_TO_GOAL
         elif state is MOVING_TO_GOAL:
             if distance > 0.1 and (old_goal is goal_pos):
-                joint_angles = joint_angles + Kp*ang_diff(joint_goal_angles, joint_angles)*dt
+                joint_angles = joint_angles + Kp * \
+                    ang_diff(joint_goal_angles, joint_angles) * dt
             else:
                 state = WAIT_FOR_NEW_GOAL
                 solution_found = False
 
         arm.update_joints(joint_angles)
-            
+
 
 def inverse_kinematics(link_lengths, joint_angles, goal_pos):
     """
@@ -69,34 +75,90 @@ def inverse_kinematics(link_lengths, joint_angles, goal_pos):
         joint_angles = joint_angles + np.matmul(J, errors)
     return joint_angles, False
 
+
+def get_random_goal():
+    from random import random
+    SAREA = 15.0
+    return [SAREA * random() - SAREA / 2.0,
+            SAREA * random() - SAREA / 2.0]
+
+
+def animation():
+    link_lengths = [1] * N_LINKS
+    joint_angles = np.array([0] * N_LINKS)
+    goal_pos = get_random_goal()
+    arm = NLinkArm(link_lengths, joint_angles, goal_pos, show_animation)
+    state = WAIT_FOR_NEW_GOAL
+    solution_found = False
+
+    i_goal = 0
+    while True:
+        old_goal = goal_pos
+        goal_pos = arm.goal
+        end_effector = arm.end_effector
+        errors, distance = distance_to_goal(end_effector, goal_pos)
+
+        # State machine to allow changing of goal before current goal has been reached
+        if state is WAIT_FOR_NEW_GOAL:
+
+            if distance > 0.1 and not solution_found:
+                joint_goal_angles, solution_found = inverse_kinematics(
+                    link_lengths, joint_angles, goal_pos)
+                if not solution_found:
+                    print("Solution could not be found.")
+                    state = WAIT_FOR_NEW_GOAL
+                    arm.goal = get_random_goal()
+                elif solution_found:
+                    state = MOVING_TO_GOAL
+        elif state is MOVING_TO_GOAL:
+            if distance > 0.1 and (old_goal is goal_pos):
+                joint_angles = joint_angles + Kp * \
+                    ang_diff(joint_goal_angles, joint_angles) * dt
+            else:
+                state = WAIT_FOR_NEW_GOAL
+                solution_found = False
+                arm.goal = get_random_goal()
+                i_goal += 1
+
+        if i_goal >= 5:
+            break
+
+        arm.update_joints(joint_angles)
+
+
 def forward_kinematics(link_lengths, joint_angles):
     x = y = 0
-    for i in range(1, N_LINKS+1):
-        x += link_lengths[i-1]*np.cos(np.sum(joint_angles[:i]))
-        y += link_lengths[i-1]*np.sin(np.sum(joint_angles[:i]))
+    for i in range(1, N_LINKS + 1):
+        x += link_lengths[i - 1] * np.cos(np.sum(joint_angles[:i]))
+        y += link_lengths[i - 1] * np.sin(np.sum(joint_angles[:i]))
     return np.array([x, y]).T
 
+
 def jacobian_inverse(link_lengths, joint_angles):
     J = np.zeros((2, N_LINKS))
     for i in range(N_LINKS):
         J[0, i] = 0
         J[1, i] = 0
         for j in range(i, N_LINKS):
-            J[0, i] -= link_lengths[j]*np.sin(np.sum(joint_angles[:j]))
-            J[1, i] += link_lengths[j]*np.cos(np.sum(joint_angles[:j]))
+            J[0, i] -= link_lengths[j] * np.sin(np.sum(joint_angles[:j]))
+            J[1, i] += link_lengths[j] * np.cos(np.sum(joint_angles[:j]))
 
     return np.linalg.pinv(J)
 
+
 def distance_to_goal(current_pos, goal_pos):
     x_diff = goal_pos[0] - current_pos[0]
     y_diff = goal_pos[1] - current_pos[1]
     return np.array([x_diff, y_diff]).T, np.math.sqrt(x_diff**2 + y_diff**2)
 
+
 def ang_diff(theta1, theta2):
     """
     Returns the difference between two angles in the range -pi to +pi
     """
-    return (theta1 - theta2 + np.pi)%(2*np.pi) - np.pi
+    return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi
+
 
 if __name__ == '__main__':
-    n_link_arm_ik()
+    main()
+    # animation()

ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py

@@ -0,0 +1,122 @@
+"""
+Inverse kinematics of a two-joint arm
+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)
+
+"""
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+# Similation parameters
+Kp = 15
+dt = 0.01
+
+# Link lengths
+l1 = l2 = 1
+
+# Set initial goal position to the initial end-effector position
+x = 2
+y = 0
+
+show_animation = True
+
+if show_animation:
+    plt.ion()
+
+
+def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
+    """
+    Computes the inverse kinematics for a planar 2DOF arm
+    """
+    while True:
+        try:
+            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)))
+
+            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)))
+
+            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")
+
+        wrist = plot_arm(theta1, theta2, x, y)
+
+        # check goal
+        d2goal = np.math.sqrt((wrist[0] - x)**2 + (wrist[1] - y)**2)
+
+        if abs(d2goal) < GOAL_TH and x is not None:
+            return theta1, theta2
+
+
+def plot_arm(theta1, theta2, x, y):
+    shoulder = np.array([0, 0])
+    elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])
+    wrist = elbow + \
+        np.array([l2 * np.cos(theta1 + theta2), l2 * np.sin(theta1 + theta2)])
+
+    if show_animation:
+        plt.cla()
+
+        plt.plot([shoulder[0], elbow[0]], [shoulder[1], elbow[1]], 'k-')
+        plt.plot([elbow[0], wrist[0]], [elbow[1], wrist[1]], 'k-')
+
+        plt.plot(shoulder[0], shoulder[1], 'ro')
+        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.xlim(-2, 2)
+        plt.ylim(-2, 2)
+
+        plt.show()
+        plt.pause(dt)
+
+    return wrist
+
+
+def ang_diff(theta1, theta2):
+    # Returns the difference between two angles in the range -pi to +pi
+    return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi
+
+
+def click(event):
+    global x, y
+    x = event.xdata
+    y = event.ydata
+
+
+def animation():
+    from random import random
+    global x, y
+    theta1 = theta2 = 0.0
+    for i in range(5):
+        x = 2.0 * random() - 1.0
+        y = 2.0 * random() - 1.0
+        theta1, theta2 = two_joint_arm(
+            GOAL_TH=0.01, theta1=theta1, theta2=theta2)
+
+
+def main():
+    fig = plt.figure()
+    fig.canvas.mpl_connect("button_press_event", click)
+    two_joint_arm()
+
+
+if __name__ == "__main__":
+    # animation()
+    main()

Localization/Kalmanfilter_basics_2.ipynb

@@ -0,0 +1,347 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# _KF Basics - Part 2_\n",
+    "-------"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    " ### Probabilistic Generative Laws\n",
+    " ------------\n",
+    "**1st Law**:\n",
+    "The belief representing the state $x_{t}$, is conditioned on all past states, measurements and controls. This can be shown mathematically by the conditional probability shown below:\n",
+    "\n",
+    "$$p(x_{t} | x_{0:t-1},z_{1:t-1},u_{1:t})$$\n",
+    "\n",
+    "1) $z_{t}$ represents the **measurement**\n",
+    "\n",
+    "2) $u_{t}$ the **motion command**\n",
+    "\n",
+    "3) $x_{t}$ the **state** (can be the position, velocity, etc) of the robot or its environment at time t.\n",
+    "\n",
+    "\n",
+    "'If we know the state $x_{t-1}$ and $u_{t}$, then knowing the states $x_{0:t-2}$, $z_{1:t-1}$ becomes immaterial through the property of **conditional independence**'. The state $x_{t-1}$ introduces a conditional independence between $x_{t}$ and $z_{1:t-1}$, $u_{1:t-1}$\n",
+    "\n",
+    "Therefore the law now holds as:\n",
+    "\n",
+    "$$p(x_{t} | x_{0:t-1},z_{1:t-1},u_{1:t})=p(x_{t} | x_{t-1},u_{t})$$\n",
+    "\n",
+    "**2nd Law**:\n",
+    "\n",
+    "If $x_{t}$ is complete, then:\n",
+    "\n",
+    "$$p(z_{t} | x-_{0:t},z_{1:t-1},u_{1:t})=p(z_{t} | x_{t})$$\n",
+    "\n",
+    "$x_{t}$ is **complete** means that the past states, controls or measurements carry no additional information to predict future.\n",
+    "\n",
+    "$x_{0:t-1}$, $z_{1:t-1}$ and $u_{1:t}$ are **conditionally independent** of $z_{t}$ given $x_{t}$ of complete.\n",
+    "\n",
+    "The filter works in two parts:\n",
+    "\n",
+    "$\\bullet$  $p(x_{t} | x_{t-1},u_{t})\\rightarrow $**State Transition Probability**\n",
+    "\n",
+    "$\\bullet$  $p(z_{t} | x_{t}) \\rightarrow $**Measurement Probability**\n",
+    "\n",
+    "\n",
+    "### Conditional dependence and independence example:\n",
+    "-------\n",
+    "\n",
+    "$\\bullet$**Independent but conditionally dependent**\n",
+    "\n",
+    "Let's say you flip two fair coins\n",
+    "\n",
+    "A - Your first coin flip is heads\n",
+    "\n",
+    "B - Your second coin flip is heads\n",
+    "\n",
+    "C - Your first two flips were the same\n",
+    "\n",
+    "\n",
+    "A and B here are independent. However, A and B are conditionally dependent given C, since if you know C then your first coin flip will inform the other one.\n",
+    "\n",
+    "$\\bullet$**Dependent but conditionally independent**\n",
+    "\n",
+    "A box contains two coins: a regular coin and one fake two-headed coin ((P(H)=1). I choose a coin at random and toss it twice. Define the following events.\n",
+    "\n",
+    "A= First coin toss results in an H.\n",
+    "\n",
+    "B= Second coin toss results in an H.\n",
+    "\n",
+    "C= Coin 1 (regular) has been selected.                                                             \n",
+    "\n",
+    "If we know A has occurred (i.e., the first coin toss has resulted in heads), we would guess that it is more likely that we have chosen Coin 2 than Coin 1. This in turn increases the conditional probability that B occurs. This suggests that A and B are not independent. On the other hand, given C (Coin 1 is selected), A and B are independent.\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "### Bayes Rule:\n",
+    "------------\n",
+    "\n",
+    "\n",
+    "Posterior = $\\frac{Likelihood*Prior}{Marginal} $\n",
+    "\n",
+    "Here,\n",
+    "\n",
+    "**Posterior** = Probability of an event occurring based on certain evidence.\n",
+    "\n",
+    "**Likelihood** = How probable is the evidence given the event.\n",
+    "\n",
+    "**Prior** = Probability of the just the event occurring without having any evidence.\n",
+    "\n",
+    "**Marginal** = Probability of the evidence given all the instances of events possible.\n",
+    "\n",
+    "\n",
+    "\n",
+    "Example:\n",
+    "\n",
+    "1% of women have breast cancer (and therefore 99% do not).\n",
+    "80% of mammograms detect breast cancer when it is there (and therefore 20% miss it).\n",
+    "9.6% of mammograms detect breast cancer when its not there (and therefore 90.4% correctly return a negative result).\n",
+    "\n",
+    "We can turn the process above into an equation, which is Bayes Theorem. Here is the equation:\n",
+    "\n",
+    "$\\displaystyle{\\Pr(\\mathrm{A}|\\mathrm{X}) = \\frac{\\Pr(\\mathrm{X}|\\mathrm{A})\\Pr(\\mathrm{A})}{\\Pr(\\mathrm{X|A})\\Pr(\\mathrm{A})+ \\Pr(\\mathrm{X | not \\ A})\\Pr(\\mathrm{not \\ A})}}$\n",
+    "\n",
+    "\n",
+    "$\\bullet$Pr(A|X) = Chance of having cancer (A) given a positive test (X). This is what we want to know: How likely is it to have cancer with a positive result? In our case it was 7.8%.\n",
+    "\n",
+    "$\\bullet$Pr(X|A) = Chance of a positive test (X) given that you had cancer (A). This is the chance of a true positive, 80% in our case.\n",
+    "\n",
+    "$\\bullet$Pr(A) = Chance of having cancer (1%).\n",
+    "\n",
+    "$\\bullet$Pr(not A) = Chance of not having cancer (99%).\n",
+    "\n",
+    "$\\bullet$Pr(X|not A) = Chance of a positive test (X) given that you didn't have cancer (~A). This is a false positive, 9.6% in our case.\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "### Bayes Filter Algorithm\n",
+    "-------\n",
+    "The basic filter algorithm is:\n",
+    "\n",
+    "for all $x_{t}$:\n",
+    "\n",
+    "1. $\\overline{bel}(x_t) = \\int p(x_t | u_t, x_{t-1}) bel(x_{t-1})dx$\n",
+    "\n",
+    "2. $bel(x_t) = \\eta p(z_t | x_t) \\overline{bel}(x_t)$\n",
+    "\n",
+    "end.\n",
+    "\n",
+    "\n",
+    "$\\rightarrow$The first step in filter is to calculate the prior for the next step that uses the bayes theorem. This is the **Prediction** step. The belief, $\\overline{bel}(x_t)$, is **before** incorporating measurement($z_{t}$) at time t=t. This is the step where the motion occurs and information is lost because the means and covariances of the gaussians are added. The RHS of the equation incorporates the law of total probability for prior calculation.\n",
+    "\n",
+    "$\\rightarrow$ This is the **Correction** or update step that calculates the belief of the robot **after** taking into account the measurement($z_{t}$) at time t=t. This is where we incorporate the sensor information for the whereabouts of the robot. We gain information here as the gaussians get multiplied here. (Multiplication of gaussian values allows the resultant to lie in between these numbers and the resultant covariance is smaller.\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "### Bayes filter localization example:\n",
+    "----\n",
+    "\n",
+    "<img src=\"bayes_filter.png\" title=\"Bayes filter\" width=\"50%\" height=\"50%\" />\n",
+    "\n",
+    "\n",
+    "Given - A robot with a sensor to detect doorways along a hallway. Also, the robot knows how the hallway looks like but doesn't know where it is in the map. \n",
+    "\n",
+    "\n",
+    "1. Initially(first scenario), it doesn't know where it is with respect to the map and hence the belief assigns equal probability to each location in the map.\n",
+    "\n",
+    "\n",
+    "2. The first sensor reading is incorporated and it shows the presence of a door. Now the robot knows how the map looks like but cannot localize yet as map has 3 doors present. Therefore it assigns equal probability to each door present. \n",
+    "\n",
+    "\n",
+    "3. The robot now moves forward. This is the prediction step and the motion causes the robot to lose some of the information and hence the variance of the gaussians increase (diagram 4.). The final belief is **convolution** of posterior from previous step and the current state after motion. Also, the means shift on the right due to the motion.\n",
+    "\n",
+    "\n",
+    "4. Again, incorporating the measurement, the sensor senses a door and this time too the possibility of door is equal for the three door. This is where the filter's magic kicks in. For the final belief (diagram 5.), the posterior calculated after sensing is mixed or **convolution** of previous posterior and measurement. It improves the robot's belief at location near to the second door. The variance **decreases** and **peaks**.\n",
+    "\n",
+    "\n",
+    "5. Finally after series of iterations of motion and correction, the robot is able to localize itself with respect to the environment.(diagram 6.)\n",
+    "\n",
+    "Do note that the robot knows the map but doesn't know where exactly it is on the map."
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "### Bayes and Kalman filter structure\n",
+    "-----\n",
+    "The basic structure and the concept remains the same as bayes filter for Kalman. The only key difference is the mathematical representation of Kalman filter. The Kalman filter is nothing but a bayesian filter that uses Gaussians.\n",
+    "\n",
+    "For a bayes filter to be a Kalman filter, **each term of belief is now a gaussian**, unlike histograms.  The basic formulation for the **bayes filter** algorithm is:\n",
+    "\n",
+    "$$\\begin{aligned} \n",
+    "\\bar {\\mathbf x} &= \\mathbf x \\ast f_{\\mathbf x}(\\bullet)\\, \\, &\\text{Prediction} \\\\\n",
+    "\\mathbf x &= \\mathcal L \\cdot \\bar{\\mathbf x}\\, \\, &\\text{Correction}\n",
+    "\\end{aligned}$$\n",
+    "\n",
+    "\n",
+    "$\\bar{\\mathbf x}$ is the *prior* \n",
+    "\n",
+    "$\\mathcal L$ is the *likelihood* of a measurement given the prior $\\bar{\\mathbf x}$\n",
+    "\n",
+    "$f_{\\mathbf x}(\\bullet)$ is the *process model* or the gaussian term that helps predict the next state like velocity\n",
+    "to track position or acceleration.\n",
+    "\n",
+    "$\\ast$ denotes *convolution*.\n",
+    "\n",
+    "\n",
+    "\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "### Kalman Gain\n",
+    "-------\n",
+    "\n",
+    "$$ x = (\\mathcal L \\bar x)$$\n",
+    "\n",
+    "Where x is posterior and $\\mathcal L$ and $\\bar x$ are gaussians.\n",
+    "\n",
+    "Therefore the mean of the posterior is given by:\n",
+    "\n",
+    "$$\n",
+    "\\mu=\\frac{\\bar\\sigma^2\\, \\mu_z + \\sigma_z^2 \\, \\bar\\mu} {\\bar\\sigma^2 + \\sigma_z^2}\n",
+    "$$\n",
+    "\n",
+    "\n",
+    "$$\\mu = \\left( \\frac{\\bar\\sigma^2}{\\bar\\sigma^2 + \\sigma_z^2}\\right) \\mu_z + \\left(\\frac{\\sigma_z^2}{\\bar\\sigma^2 + \\sigma_z^2}\\right)\\bar\\mu$$\n",
+    "\n",
+    "In this form it is easy to see that we are scaling the measurement and the prior by weights: \n",
+    "\n",
+    "$$\\mu = W_1 \\mu_z + W_2 \\bar\\mu$$\n",
+    "\n",
+    "\n",
+    "The weights sum to one because the denominator is a normalization term. We introduce a new term, $K=W_1$, giving us:\n",
+    "\n",
+    "$$\\begin{aligned}\n",
+    "\\mu &= K \\mu_z + (1-K) \\bar\\mu\\\\\n",
+    "&= \\bar\\mu + K(\\mu_z - \\bar\\mu)\n",
+    "\\end{aligned}$$\n",
+    "\n",
+    "where\n",
+    "\n",
+    "$$K = \\frac {\\bar\\sigma^2}{\\bar\\sigma^2 + \\sigma_z^2}$$\n",
+    "\n",
+    "The variance in terms of the Kalman gain:\n",
+    "\n",
+    "$$\\begin{aligned}\n",
+    "\\sigma^2 &= \\frac{\\bar\\sigma^2 \\sigma_z^2 } {\\bar\\sigma^2 + \\sigma_z^2} \\\\\n",
+    "&= K\\sigma_z^2 \\\\\n",
+    "&= (1-K)\\bar\\sigma^2 \n",
+    "\\end{aligned}$$\n",
+    "\n",
+    "\n",
+    "**$K$ is the *Kalman gain*. It's the crux of the Kalman filter. It is a scaling term that chooses a value partway between $\\mu_z$ and $\\bar\\mu$.**"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "### Kalman Filter - Univariate and Multivariate\n",
+    "-----\n",
+    "\n",
+    "<u>**Prediction**</u>\n",
+    "\n",
+    "$\\begin{array}{|l|l|l|}\n",
+    "\\hline\n",
+    "\\text{Univariate} & \\text{Univariate} & \\text{Multivariate}\\\\\n",
+    "& \\text{(Kalman form)} & \\\\\n",
+    "\\hline\n",
+    "\\bar \\mu = \\mu + \\mu_{f_x} & \\bar x = x + dx & \\bar{\\mathbf x} = \\mathbf{Fx} + \\mathbf{Bu}\\\\\n",
+    "\\bar\\sigma^2 = \\sigma_x^2 + \\sigma_{f_x}^2 & \\bar P = P + Q & \\bar{\\mathbf P} = \\mathbf{FPF}^\\mathsf T + \\mathbf Q \\\\\n",
+    "\\hline\n",
+    "\\end{array}$\n",
+    "\n",
+    "$\\mathbf x,\\, \\mathbf P$ are the state mean and covariance. They correspond to $x$ and $\\sigma^2$.\n",
+    "\n",
+    "$\\mathbf F$ is the *state transition function*. When multiplied by $\\bf x$ it computes the prior. \n",
+    "\n",
+    "$\\mathbf Q$ is the process covariance. It corresponds to $\\sigma^2_{f_x}$.\n",
+    "\n",
+    "$\\mathbf B$ and $\\mathbf u$ are model control inputs to the system.\n",
+    "\n",
+    "<u>**Correction**</u>\n",
+    "\n",
+    "$\\begin{array}{|l|l|l|}\n",
+    "\\hline\n",
+    "\\text{Univariate} & \\text{Univariate} & \\text{Multivariate}\\\\\n",
+    "& \\text{(Kalman form)} & \\\\\n",
+    "\\hline\n",
+    "& y = z - \\bar x & \\mathbf y = \\mathbf z - \\mathbf{H\\bar x} \\\\\n",
+    "& K = \\frac{\\bar P}{\\bar P+R}&\n",
+    "\\mathbf K = \\mathbf{\\bar{P}H}^\\mathsf T (\\mathbf{H\\bar{P}H}^\\mathsf T + \\mathbf R)^{-1} \\\\\n",
+    "\\mu=\\frac{\\bar\\sigma^2\\, \\mu_z + \\sigma_z^2 \\, \\bar\\mu} {\\bar\\sigma^2 + \\sigma_z^2} & x = \\bar x + Ky & \\mathbf x = \\bar{\\mathbf x} + \\mathbf{Ky} \\\\\n",
+    "\\sigma^2 = \\frac{\\sigma_1^2\\sigma_2^2}{\\sigma_1^2+\\sigma_2^2} & P = (1-K)\\bar P &\n",
+    "\\mathbf P = (\\mathbf I - \\mathbf{KH})\\mathbf{\\bar{P}} \\\\\n",
+    "\\hline\n",
+    "\\end{array}$\n",
+    "\n",
+    "$\\mathbf H$ is the measurement function.\n",
+    "\n",
+    "$\\mathbf z,\\, \\mathbf R$ are the measurement mean and noise covariance. They correspond to $z$ and $\\sigma_z^2$ in the univariate filter. \n",
+    "$\\mathbf y$ and $\\mathbf K$ are the residual and Kalman gain. \n",
+    "\n",
+    "The details will be different than the univariate filter because these are vectors and matrices, but the concepts are exactly the same: \n",
+    "\n",
+    "-  Use a Gaussian to represent our estimate of the state and error\n",
+    "-  Use a Gaussian to represent the measurement and its error\n",
+    "-  Use a Gaussian to represent the process model\n",
+    "-  Use the process model to predict the next state (the prior)\n",
+    "-  Form an estimate part way between the measurement and the prior"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "### References:\n",
+    "-------\n",
+    "\n",
+    "1. Roger Labbe's [repo](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python) on Kalman Filters. (Majority of text in the notes are from this)\n",
+    "\n",
+    "\n",
+    "\n",
+    "2. Probabilistic Robotics by Sebastian Thrun, Wolfram Burgard and Dieter Fox, MIT Press.\n"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.6.5"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}

Localization/extended_kalman_filter/extended_kalman_filter.py

@@ -80,12 +80,25 @@ def observation_model(x):
 
 
 def jacobF(x, u):
-    # Jacobian of Motion Model
+    """
+    Jacobian of Motion Model
+
+    motion model
+    x_{t+1} = x_t+v*dt*cos(yaw)
+    y_{t+1} = y_t+v*dt*sin(yaw)
+    yaw_{t+1} = yaw_t+omega*dt
+    v_{t+1} = v{t}
+    so
+    dx/dyaw = -v*dt*sin(yaw)
+    dx/dv = dt*cos(yaw)
+    dy/dyaw = v*dt*cos(yaw)
+    dy/dv = dt*sin(yaw)
+    """
     yaw = x[2, 0]
-    u1 = u[0, 0]
+    v = u[0, 0]
     jF = np.matrix([
-        [1.0, 0.0, -DT * u1 * math.sin(yaw), DT * u1 * math.cos(yaw)],
-        [0.0, 1.0, DT * math.cos(yaw), DT * math.sin(yaw)],
+        [1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)],
+        [0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)],
         [0.0, 0.0, 1.0, 0.0],
         [0.0, 0.0, 0.0, 1.0]])
 

Localization/extended_kalman_filter/notebook.ipynb

@@ -0,0 +1,48 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Extended Kalman Filter Localization\n",
+    "\n",
+    "Code; [PythonRobotics/extended\\_kalman\\_filter\\.py at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/extended_kalman_filter/extended_kalman_filter.py)"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Kalman Filter"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Ref"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.6.6"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}

Localization/histogram_filter/histogram_filter.py

@@ -0,0 +1,258 @@
+"""
+
+Histogram Filter 2D localization example
+
+
+In this simulation, x,y are unknown, yaw is known.
+
+Initial position is not needed.
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+"""
+
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+import copy
+from scipy.stats import norm
+from scipy.ndimage import gaussian_filter
+
+# Parameters
+EXTEND_AREA = 10.0  # [m] grid map extention length
+SIM_TIME = 50.0  # simulation time [s]
+DT = 0.1  # time tick [s]
+MAX_RANGE = 10.0  # maximum observation range
+MOTION_STD = 1.0  # standard deviation for motion gaussian distribution
+RANGE_STD = 3.0  # standard deviation for observation gaussian distribution
+
+# grid map param
+XY_RESO = 0.5  # xy grid resolution
+MINX = -15.0
+MINY = -5.0
+MAXX = 15.0
+MAXY = 25.0
+
+# simulation paramters
+NOISE_RANGE = 2.0  # [m] 1σ range noise parameter
+NOISE_SPEED = 0.5  # [m/s] 1σ speed noise parameter
+
+
+show_animation = True
+
+
+class grid_map():
+
+    def __init__(self):
+        self.data = None
+        self.xyreso = None
+        self.minx = None
+        self.miny = None
+        self.maxx = None
+        self.maxx = None
+        self.xw = None
+        self.yw = None
+        self.dx = 0.0  # movement distance
+        self.dy = 0.0  # movement distance
+
+
+def histogram_filter_localization(gmap, u, z, yaw):
+
+    gmap = motion_update(gmap, u, yaw)
+
+    gmap = observation_update(gmap, z, RANGE_STD)
+
+    return gmap
+
+
+def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std):
+
+    # predicted range
+    x = ix * gmap.xyreso + gmap.minx
+    y = iy * gmap.xyreso + gmap.miny
+    d = math.sqrt((x - z[iz, 1])**2 + (y - z[iz, 2])**2)
+
+    # likelihood
+    pdf = (1.0 - norm.cdf(abs(d - z[iz, 0]), 0.0, std))
+
+    return pdf
+
+
+def observation_update(gmap, z, std):
+
+    for iz in range(z.shape[0]):
+        for ix in range(gmap.xw):
+            for iy in range(gmap.yw):
+                gmap.data[ix][iy] *= calc_gaussian_observation_pdf(
+                    gmap, z, iz, ix, iy, std)
+
+    gmap = normalize_probability(gmap)
+
+    return gmap
+
+
+def calc_input():
+    v = 1.0  # [m/s]
+    yawrate = 0.1  # [rad/s]
+    u = np.matrix([v, yawrate]).T
+    return u
+
+
+def motion_model(x, u):
+
+    F = np.matrix([[1.0, 0, 0, 0],
+                   [0, 1.0, 0, 0],
+                   [0, 0, 1.0, 0],
+                   [0, 0, 0, 0]])
+
+    B = np.matrix([[DT * math.cos(x[2, 0]), 0],
+                   [DT * math.sin(x[2, 0]), 0],
+                   [0.0, DT],
+                   [1.0, 0.0]])
+
+    x = F * x + B * u
+
+    return x
+
+
+def draw_heatmap(data, mx, my):
+    maxp = max([max(igmap) for igmap in data])
+    plt.pcolor(mx, my, data, vmax=maxp, cmap=plt.cm.Blues)
+    plt.axis("equal")
+
+
+def observation(xTrue, u, RFID):
+
+    xTrue = motion_model(xTrue, u)
+
+    z = np.matrix(np.zeros((0, 3)))
+
+    for i in range(len(RFID[:, 0])):
+
+        dx = xTrue[0, 0] - RFID[i, 0]
+        dy = xTrue[1, 0] - RFID[i, 1]
+        d = math.sqrt(dx**2 + dy**2)
+        if d <= MAX_RANGE:
+            # add noise to range observation
+            dn = d + np.random.randn() * NOISE_RANGE
+            zi = np.matrix([dn, RFID[i, 0], RFID[i, 1]])
+            z = np.vstack((z, zi))
+
+    # add noise to speed
+    ud = u[:, :]
+    ud[0] += np.random.randn() * NOISE_SPEED
+
+    return xTrue, z, ud
+
+
+def normalize_probability(gmap):
+
+    sump = sum([sum(igmap) for igmap in gmap.data])
+
+    for ix in range(gmap.xw):
+        for iy in range(gmap.yw):
+            gmap.data[ix][iy] /= sump
+
+    return gmap
+
+
+def init_gmap(xyreso, minx, miny, maxx, maxy):
+
+    gmap = grid_map()
+
+    gmap.xyreso = xyreso
+    gmap.minx = minx
+    gmap.miny = miny
+    gmap.maxx = maxx
+    gmap.maxy = maxy
+    gmap.xw = int(round((gmap.maxx - gmap.minx) / gmap.xyreso))
+    gmap.yw = int(round((gmap.maxy - gmap.miny) / gmap.xyreso))
+
+    gmap.data = [[1.0 for i in range(gmap.yw)] for i in range(gmap.xw)]
+    gmap = normalize_probability(gmap)
+
+    return gmap
+
+
+def map_shift(gmap, xshift, yshift):
+
+    tgmap = copy.deepcopy(gmap.data)
+
+    for ix in range(gmap.xw):
+        for iy in range(gmap.yw):
+            nix = ix + xshift
+            niy = iy + yshift
+
+            if nix >= 0 and nix < gmap.xw and niy >= 0 and niy < gmap.yw:
+                gmap.data[ix + xshift][iy + yshift] = tgmap[ix][iy]
+
+    return gmap
+
+
+def motion_update(gmap, u, yaw):
+
+    gmap.dx += DT * math.cos(yaw) * u[0]
+    gmap.dy += DT * math.sin(yaw) * u[0]
+
+    xshift = gmap.dx // gmap.xyreso
+    yshift = gmap.dy // gmap.xyreso
+
+    if abs(xshift) >= 1.0 or abs(yshift) >= 1.0:  # map should be shifted
+        gmap = map_shift(gmap, int(xshift), int(yshift))
+        gmap.dx -= xshift * gmap.xyreso
+        gmap.dy -= yshift * gmap.xyreso
+
+    gmap.data = gaussian_filter(gmap.data, sigma=MOTION_STD)
+
+    return gmap
+
+
+def calc_grid_index(gmap):
+    mx, my = np.mgrid[slice(gmap.minx - gmap.xyreso / 2.0, gmap.maxx + gmap.xyreso / 2.0, gmap.xyreso),
+                      slice(gmap.miny - gmap.xyreso / 2.0, gmap.maxy + gmap.xyreso / 2.0, gmap.xyreso)]
+
+    return mx, my
+
+
+def main():
+    print(__file__ + " start!!")
+
+    # RFID positions [x, y]
+    RFID = np.array([[10.0, 0.0],
+                     [10.0, 10.0],
+                     [0.0, 15.0],
+                     [-5.0, 20.0]])
+
+    time = 0.0
+
+    xTrue = np.matrix(np.zeros((4, 1)))
+    gmap = init_gmap(XY_RESO, MINX, MINY, MAXX, MAXY)
+    mx, my = calc_grid_index(gmap)  # for grid map visualization
+
+    while SIM_TIME >= time:
+        time += DT
+        print("Time:", time)
+
+        u = calc_input()
+
+        yaw = xTrue[2, 0]  # Orientation is known
+        xTrue, z, ud = observation(xTrue, u, RFID)
+
+        gmap = histogram_filter_localization(gmap, u, z, yaw)
+
+        if show_animation:
+            plt.cla()
+            draw_heatmap(gmap.data, mx, my)
+            plt.plot(xTrue[0, :], xTrue[1, :], "xr")
+            plt.plot(RFID[:, 0], RFID[:, 1], ".k")
+            for i in range(z.shape[0]):
+                plt.plot([xTrue[0, :], z[i, 1]], [
+                         xTrue[1, :], z[i, 2]], "-k")
+            plt.title("Time[s]:" + str(time)[0: 4])
+            plt.pause(0.1)
+
+    print("Done")
+
+
+if __name__ == '__main__':
+    main()

Mapping/circle_fitting/circle_fitting.py

@@ -0,0 +1,138 @@
+"""
+
+Object shape recognition with circle fitting
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+"""
+
+import matplotlib.pyplot as plt
+import math
+import random
+import numpy as np
+
+show_animation = True
+
+
+def circle_fitting(x, y):
+    """
+    Circle Fitting with least squared
+        input: point x-y positions
+        output  cxe x center position
+                cye y center position
+                re  radius of circle
+                error: prediction error
+    """
+
+    sumx = sum(x)
+    sumy = sum(y)
+    sumx2 = sum([ix ** 2 for ix in x])
+    sumy2 = sum([iy ** 2 for iy in y])
+    sumxy = sum([ix * iy for (ix, iy) in zip(x, y)])
+
+    F = np.array([[sumx2, sumxy, sumx],
+                  [sumxy, sumy2, sumy],
+                  [sumx, sumy, len(x)]])
+
+    G = np.array([[-sum([ix ** 3 + ix * iy ** 2 for (ix, iy) in zip(x, y)])],
+                  [-sum([ix ** 2 * iy + iy ** 3 for (ix, iy) in zip(x, y)])],
+                  [-sum([ix ** 2 + iy ** 2 for (ix, iy) in zip(x, y)])]])
+
+    T = np.linalg.inv(F).dot(G)
+
+    cxe = float(T[0] / -2)
+    cye = float(T[1] / -2)
+    re = math.sqrt(cxe**2 + cye**2 - T[2])
+
+    error = sum([np.hypot(cxe - ix, cye - iy) - re for (ix, iy) in zip(x, y)])
+
+    return (cxe, cye, re, error)
+
+
+def get_sample_points(cx, cy, cr, angle_reso):
+    x, y, angle, r = [], [], [], []
+
+    # points sampling
+    for theta in np.arange(0.0, 2.0 * math.pi, angle_reso):
+        nx = cx + cr * math.cos(theta)
+        ny = cy + cr * math.sin(theta)
+        nangle = math.atan2(ny, nx)
+        nr = math.hypot(nx, ny) * random.uniform(0.95, 1.05)
+
+        x.append(nx)
+        y.append(ny)
+        angle.append(nangle)
+        r.append(nr)
+
+    # ray casting filter
+    rx, ry = ray_casting_filter(x, y, angle, r, angle_reso)
+
+    return rx, ry
+
+
+def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
+    rx, ry = [], []
+    rangedb = [float("inf") for _ in range(
+        int(math.floor((math.pi * 2.0) / angle_reso)) + 1)]
+
+    for i in range(len(thetal)):
+        angleid = math.floor(thetal[i] / angle_reso)
+
+        if rangedb[angleid] > rangel[i]:
+            rangedb[angleid] = rangel[i]
+
+    for i in range(len(rangedb)):
+        t = i * angle_reso
+        if rangedb[i] != float("inf"):
+            rx.append(rangedb[i] * math.cos(t))
+            ry.append(rangedb[i] * math.sin(t))
+
+    return rx, ry
+
+
+def plot_circle(x, y, size, color="-b"):
+    deg = list(range(0, 360, 5))
+    deg.append(0)
+    xl = [x + size * math.cos(math.radians(d)) for d in deg]
+    yl = [y + size * math.sin(math.radians(d)) for d in deg]
+    plt.plot(xl, yl, color)
+
+
+def main():
+
+    # simulation parameters
+    simtime = 15.0  # simulation time
+    dt = 1.0  # time tick
+
+    cx = -2.0  # initial x position of obstacle
+    cy = -8.0  # initial y position of obstacle
+    cr = 1.0  # obstacle radious
+    theta = math.radians(30.0)  # obstacle moving direction
+    angle_reso = math.radians(3.0)  # sensor angle resolution
+
+    time = 0.0
+    while time <= simtime:
+        time += dt
+
+        cx += math.cos(theta)
+        cy += math.cos(theta)
+
+        x, y = get_sample_points(cx, cy, cr, angle_reso)
+
+        ex, ey, er, error = circle_fitting(x, y)
+        print("Error:", error)
+
+        if show_animation:
+            plt.cla()
+            plt.axis("equal")
+            plt.plot(0.0, 0.0, "*r")
+            plot_circle(cx, cy, cr)
+            plt.plot(x, y, "xr")
+            plot_circle(ex, ey, er, "-r")
+            plt.pause(dt)
+
+    print("Done")
+
+
+if __name__ == '__main__':
+    main()

Mapping/kmeans_clustering/kmeans_clustering.py

@@ -0,0 +1,179 @@
+"""
+
+Object clustering with k-means algorithm
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+"""
+
+import numpy as np
+import math
+import matplotlib.pyplot as plt
+import random
+
+show_animation = True
+
+
+class Clusters:
+
+    def __init__(self, x, y, nlabel):
+        self.x = x
+        self.y = y
+        self.ndata = len(self.x)
+        self.nlabel = nlabel
+        self.labels = [random.randint(0, nlabel - 1)
+                       for _ in range(self.ndata)]
+        self.cx = [0.0 for _ in range(nlabel)]
+        self.cy = [0.0 for _ in range(nlabel)]
+
+
+def kmeans_clustering(rx, ry, nc):
+
+    clusters = Clusters(rx, ry, nc)
+    clusters = calc_centroid(clusters)
+
+    MAX_LOOP = 10
+    DCOST_TH = 0.1
+    pcost = 100.0
+    for loop in range(MAX_LOOP):
+        #  print("Loop:", loop)
+        clusters, cost = update_clusters(clusters)
+        clusters = calc_centroid(clusters)
+
+        dcost = abs(cost - pcost)
+        if dcost < DCOST_TH:
+            break
+        pcost = cost
+
+    return clusters
+
+
+def calc_centroid(clusters):
+
+    for ic in range(clusters.nlabel):
+        x, y = calc_labeled_points(ic, clusters)
+        ndata = len(x)
+        clusters.cx[ic] = sum(x) / ndata
+        clusters.cy[ic] = sum(y) / ndata
+
+    return clusters
+
+
+def update_clusters(clusters):
+    cost = 0.0
+
+    for ip in range(clusters.ndata):
+        px = clusters.x[ip]
+        py = clusters.y[ip]
+
+        dx = [icx - px for icx in clusters.cx]
+        dy = [icy - py for icy in clusters.cy]
+
+        dlist = [math.sqrt(idx**2 + idy**2) for (idx, idy) in zip(dx, dy)]
+        mind = min(dlist)
+        min_id = dlist.index(mind)
+        clusters.labels[ip] = min_id
+        cost += min_id
+
+    return clusters, cost
+
+
+def calc_labeled_points(ic, clusters):
+
+    inds = np.array([i for i in range(clusters.ndata)
+                     if clusters.labels[i] == ic])
+    tx = np.array(clusters.x)
+    ty = np.array(clusters.y)
+
+    x = tx[inds]
+    y = ty[inds]
+
+    return x, y
+
+
+def calc_raw_data(cx, cy, npoints, rand_d):
+
+    rx, ry = [], []
+
+    for (icx, icy) in zip(cx, cy):
+        for _ in range(npoints):
+            rx.append(icx + rand_d * (random.random() - 0.5))
+            ry.append(icy + rand_d * (random.random() - 0.5))
+
+    return rx, ry
+
+
+def update_positions(cx, cy):
+
+    DX1 = 0.4
+    DY1 = 0.5
+
+    cx[0] += DX1
+    cy[0] += DY1
+
+    DX2 = -0.3
+    DY2 = -0.5
+
+    cx[1] += DX2
+    cy[1] += DY2
+
+    return cx, cy
+
+
+def calc_association(cx, cy, clusters):
+
+    inds = []
+
+    for ic in range(len(cx)):
+        tcx = cx[ic]
+        tcy = cy[ic]
+
+        dx = [icx - tcx for icx in clusters.cx]
+        dy = [icy - tcy for icy in clusters.cy]
+
+        dlist = [math.sqrt(idx**2 + idy**2) for (idx, idy) in zip(dx, dy)]
+        min_id = dlist.index(min(dlist))
+        inds.append(min_id)
+
+    return inds
+
+
+def main():
+    print(__file__ + " start!!")
+
+    cx = [0.0, 8.0]
+    cy = [0.0, 8.0]
+    npoints = 10
+    rand_d = 3.0
+    ncluster = 2
+    sim_time = 15.0
+    dt = 1.0
+    time = 0.0
+
+    while time <= sim_time:
+        print("Time:", time)
+        time += dt
+
+        # simulate objects
+        cx, cy = update_positions(cx, cy)
+        rx, ry = calc_raw_data(cx, cy, npoints, rand_d)
+
+        clusters = kmeans_clustering(rx, ry, ncluster)
+
+        # for animation
+        if show_animation:
+            plt.cla()
+            inds = calc_association(cx, cy, clusters)
+            for ic in inds:
+                x, y = calc_labeled_points(ic, clusters)
+                plt.plot(x, y, "x")
+            plt.plot(cx, cy, "o")
+            plt.xlim(-2.0, 10.0)
+            plt.ylim(-2.0, 10.0)
+            plt.pause(dt)
+
+    print("Done")
+
+
+if __name__ == '__main__':
+    main()

Mapping/rectangle_fitting/rectangle_fitting.py

@@ -0,0 +1,138 @@
+"""
+
+Object shape recognition with rectangle fitting
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+"""
+
+import matplotlib.pyplot as plt
+import math
+import random
+import numpy as np
+
+show_animation = True
+
+
+def circle_fitting(x, y):
+    """
+    Circle Fitting with least squared
+        input: point x-y positions
+        output  cxe x center position
+                cye y center position
+                re  radius of circle
+                error: prediction error
+    """
+
+    sumx = sum(x)
+    sumy = sum(y)
+    sumx2 = sum([ix ** 2 for ix in x])
+    sumy2 = sum([iy ** 2 for iy in y])
+    sumxy = sum([ix * iy for (ix, iy) in zip(x, y)])
+
+    F = np.array([[sumx2, sumxy, sumx],
+                  [sumxy, sumy2, sumy],
+                  [sumx, sumy, len(x)]])
+
+    G = np.array([[-sum([ix ** 3 + ix * iy ** 2 for (ix, iy) in zip(x, y)])],
+                  [-sum([ix ** 2 * iy + iy ** 3 for (ix, iy) in zip(x, y)])],
+                  [-sum([ix ** 2 + iy ** 2 for (ix, iy) in zip(x, y)])]])
+
+    T = np.linalg.inv(F).dot(G)
+
+    cxe = float(T[0] / -2)
+    cye = float(T[1] / -2)
+    re = math.sqrt(cxe**2 + cye**2 - T[2])
+
+    error = sum([np.hypot(cxe - ix, cye - iy) - re for (ix, iy) in zip(x, y)])
+
+    return (cxe, cye, re, error)
+
+
+def get_sample_points(cx, cy, cr, angle_reso):
+    x, y, angle, r = [], [], [], []
+
+    # points sampling
+    for theta in np.arange(0.0, 2.0 * math.pi, angle_reso):
+        nx = cx + cr * math.cos(theta)
+        ny = cy + cr * math.sin(theta)
+        nangle = math.atan2(ny, nx)
+        nr = math.hypot(nx, ny) * random.uniform(0.95, 1.05)
+
+        x.append(nx)
+        y.append(ny)
+        angle.append(nangle)
+        r.append(nr)
+
+    # ray casting filter
+    rx, ry = ray_casting_filter(x, y, angle, r, angle_reso)
+
+    return rx, ry
+
+
+def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
+    rx, ry = [], []
+    rangedb = [float("inf") for _ in range(
+        int(math.floor((math.pi * 2.0) / angle_reso)) + 1)]
+
+    for i in range(len(thetal)):
+        angleid = math.floor(thetal[i] / angle_reso)
+
+        if rangedb[angleid] > rangel[i]:
+            rangedb[angleid] = rangel[i]
+
+    for i in range(len(rangedb)):
+        t = i * angle_reso
+        if rangedb[i] != float("inf"):
+            rx.append(rangedb[i] * math.cos(t))
+            ry.append(rangedb[i] * math.sin(t))
+
+    return rx, ry
+
+
+def plot_circle(x, y, size, color="-b"):
+    deg = list(range(0, 360, 5))
+    deg.append(0)
+    xl = [x + size * math.cos(math.radians(d)) for d in deg]
+    yl = [y + size * math.sin(math.radians(d)) for d in deg]
+    plt.plot(xl, yl, color)
+
+
+def main():
+
+    # simulation parameters
+    simtime = 15.0  # simulation time
+    dt = 1.0  # time tick
+
+    cx = -2.0  # initial x position of obstacle
+    cy = -8.0  # initial y position of obstacle
+    cr = 1.0  # obstacle radious
+    theta = math.radians(30.0)  # obstacle moving direction
+    angle_reso = math.radians(3.0)  # sensor angle resolution
+
+    time = 0.0
+    while time <= simtime:
+        time += dt
+
+        cx += math.cos(theta)
+        cy += math.cos(theta)
+
+        x, y = get_sample_points(cx, cy, cr, angle_reso)
+
+        ex, ey, er, error = circle_fitting(x, y)
+        print("Error:", error)
+
+        if show_animation:
+            plt.cla()
+            plt.axis("equal")
+            plt.plot(0.0, 0.0, "*r")
+            plot_circle(cx, cy, cr)
+            plt.plot(x, y, "xr")
+            plot_circle(ex, ey, er, "-r")
+            plt.pause(dt)
+
+    print("Done")
+
+
+if __name__ == '__main__':
+    main()

PathPlanning/AStar/a_star.py

@@ -1,7 +1,12 @@
 """
+
 A* grid based planning
 
 author: Atsushi Sakai(@Atsushi_twi)
+        Nikos Kanargias (nkana@tee.gr)
+
+See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
+
 """
 
 import matplotlib.pyplot as plt
@@ -59,9 +64,8 @@ def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
 
     while 1:
         c_id = min(
-            openset, key=lambda o: openset[o].cost + calc_h(ngoal, openset[o].x, openset[o].y))
+            openset, key=lambda o: openset[o].cost + calc_heuristic(ngoal, openset[o]))
         current = openset[c_id]
-        #  print("current", current)
 
         # show graph
         if show_animation:
@@ -82,31 +86,32 @@ def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
 
         # expand search grid based on motion model
         for i in range(len(motion)):
-            node = Node(current.x + motion[i][0], current.y + motion[i][1],
+            node = Node(current.x + motion[i][0],
+                        current.y + motion[i][1],
                         current.cost + motion[i][2], c_id)
             n_id = calc_index(node, xw, minx, miny)
 
+            if n_id in closedset:
+                continue
+
             if not verify_node(node, obmap, minx, miny, maxx, maxy):
                 continue
 
-            if n_id in closedset:
-                continue
-            # Otherwise if it is already in the open set
-            if n_id in openset:
-                if openset[n_id].cost > node.cost:
-                    openset[n_id].cost = node.cost
-                    openset[n_id].pind = c_id
+            if n_id not in openset:
+                openset[n_id] = node  # Discover a new node
             else:
-                openset[n_id] = node
+                if openset[n_id].cost >= node.cost:
+                    # This path is the best until now. record it!
+                    openset[n_id] = node
 
     rx, ry = calc_fianl_path(ngoal, closedset, reso)
 
     return rx, ry
 
 
-def calc_h(ngoal, x, y):
-    w = 10.0  # weight of heuristic
-    d = w * math.sqrt((ngoal.x - x)**2 + (ngoal.y - y)**2)
+def calc_heuristic(n1, n2):
+    w = 1.0  # weight of heuristic
+    d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
     return d
 
 

PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py

@@ -0,0 +1,598 @@
+"""
+Batch Informed Trees based path planning:
+Uses a heuristic to efficiently search increasingly dense
+RGGs while reusing previous information. Provides faster
+convergence that RRT*, Informed RRT* and other sampling based
+methods.
+
+Uses lazy connecting by combining sampling based methods and A*
+like incremental graph search algorithms.
+
+author: Karan Chawla(@karanchawla)
+        Atsushi Sakai(@Atsushi_twi)
+
+Reference: https://arxiv.org/abs/1405.5848
+"""
+
+import random
+import numpy as np
+import math
+import matplotlib.pyplot as plt
+
+show_animation = True
+
+
+class RTree(object):
+    # Class to represent the explicit tree created
+    # while sampling through the state space
+
+    def __init__(self, start=[0, 0], lowerLimit=[0, 0], upperLimit=[10, 10], resolution=1):
+
+        self.vertices = dict()
+        self.edges = []
+        self.start = start
+        self.lowerLimit = lowerLimit
+        self.upperLimit = upperLimit
+        self.dimension = len(lowerLimit)
+        self.num_cells = [0] * self.dimension
+        self.resolution = resolution
+        # compute the number of grid cells based on the limits and
+        # resolution given
+        for idx in range(self.dimension):
+            self.num_cells[idx] = np.ceil(
+                (upperLimit[idx] - lowerLimit[idx]) / resolution)
+
+        vertex_id = self.realWorldToNodeId(start)
+        self.vertices[vertex_id] = []
+
+    def getRootId(self):
+        # return the id of the root of the tree
+        return 0
+
+    def addVertex(self, vertex):
+        # add a vertex to the tree
+        vertex_id = self.realWorldToNodeId(vertex)
+        self.vertices[vertex_id] = []
+        return vertex_id
+
+    def addEdge(self, v, x):
+        # create an edge between v and x vertices
+        if (v, x) not in self.edges:
+            self.edges.append((v, x))
+        # since the tree is undirected
+        self.vertices[v].append(x)
+        self.vertices[x].append(v)
+
+    def realCoordsToGridCoord(self, real_coord):
+        # convert real world coordinates to grid space
+        # depends on the resolution of the grid
+        # the output is the same as real world coords if the resolution
+        # is set to 1
+        coord = [0] * self.dimension
+        for i in range(len(coord)):
+            start = self.lowerLimit[i]  # start of the grid space
+            coord[i] = np.around((real_coord[i] - start) / self.resolution)
+        return coord
+
+    def gridCoordinateToNodeId(self, coord):
+        # This function maps a grid coordinate to a unique
+        # node id
+        nodeId = 0
+        for i in range(len(coord) - 1, -1, -1):
+            product = 1
+            for j in range(0, i):
+                product = product * self.num_cells[j]
+            nodeId = nodeId + coord[i] * product
+        return nodeId
+
+    def realWorldToNodeId(self, real_coord):
+        # first convert the given coordinates to grid space and then
+        # convert the grid space coordinates to a unique node id
+        return self.gridCoordinateToNodeId(self.realCoordsToGridCoord(real_coord))
+
+    def gridCoordToRealWorldCoord(self, coord):
+        # This function smaps a grid coordinate in discrete space
+        # to a configuration in the full configuration space
+        config = [0] * self.dimension
+        for i in range(0, len(coord)):
+            # start of the real world / configuration space
+            start = self.lowerLimit[i]
+            # step from the coordinate in the grid
+            grid_step = self.resolution * coord[i]
+            config[i] = start + grid_step
+        return config
+
+    def nodeIdToGridCoord(self, node_id):
+        # This function maps a node id to the associated
+        # grid coordinate
+        coord = [0] * len(self.lowerLimit)
+        for i in range(len(coord) - 1, -1, -1):
+            # Get the product of the grid space maximums
+            prod = 1
+            for j in range(0, i):
+                prod = prod * self.num_cells[j]
+            coord[i] = np.floor(node_id / prod)
+            node_id = node_id - (coord[i] * prod)
+        return coord
+
+    def nodeIdToRealWorldCoord(self, nid):
+        # This function maps a node in discrete space to a configuraiton
+        # in the full configuration space
+        return self.gridCoordToRealWorldCoord(self.nodeIdToGridCoord(nid))
+
+# Uses Batch Informed Trees to find a path from start to goal
+
+
+class BITStar(object):
+
+    def __init__(self, start, goal,
+                 obstacleList, randArea, eta=2.0,
+                 maxIter=80):
+        self.start = start
+        self.goal = goal
+
+        self.minrand = randArea[0]
+        self.maxrand = randArea[1]
+        self.maxIter = maxIter
+        self.obstacleList = obstacleList
+
+        self.vertex_queue = []
+        self.edge_queue = []
+        self.samples = dict()
+        self.g_scores = dict()
+        self.f_scores = dict()
+        self.nodes = dict()
+        self.r = float('inf')
+        self.eta = eta  # tunable parameter
+        self.unit_ball_measure = 1
+        self.old_vertices = []
+
+        # initialize tree
+        lowerLimit = [randArea[0], randArea[0]]
+        upperLimit = [randArea[1], randArea[1]]
+        self.tree = RTree(start=start, lowerLimit=lowerLimit,
+                          upperLimit=upperLimit, resolution=0.01)
+
+    def setup_planning(self):
+        self.startId = self.tree.realWorldToNodeId(self.start)
+        self.goalId = self.tree.realWorldToNodeId(self.goal)
+
+        # add goal to the samples
+        self.samples[self.goalId] = self.goal
+        self.g_scores[self.goalId] = float('inf')
+        self.f_scores[self.goalId] = 0
+
+        # add the start id to the tree
+        self.tree.addVertex(self.start)
+        self.g_scores[self.startId] = 0
+        self.f_scores[self.startId] = self.computeHeuristicCost(
+            self.startId, self.goalId)
+
+        # max length we expect to find in our 'informed' sample space, starts as infinite
+        cBest = self.g_scores[self.goalId]
+
+        # Computing the sampling space
+        cMin = math.sqrt(pow(self.start[0] - self.goal[1], 2) +
+                         pow(self.start[0] - self.goal[1], 2)) / 1.5
+        xCenter = np.matrix([[(self.start[0] + self.goal[0]) / 2.0],
+                             [(self.goal[1] - self.start[1]) / 2.0], [0]])
+        a1 = np.matrix([[(self.goal[0] - self.start[0]) / cMin],
+                        [(self.goal[1] - self.start[1]) / cMin], [0]])
+        etheta = math.atan2(a1[1], a1[0])
+        # first column of idenity matrix transposed
+        id1_t = np.matrix([1.0, 0.0, 0.0])
+        M = np.dot(a1, id1_t)
+        U, S, Vh = np.linalg.svd(M, 1, 1)
+        C = np.dot(np.dot(U, np.diag(
+            [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh)
+
+        self.samples.update(self.informedSample(
+            200, cBest, cMin, xCenter, C))
+
+        return etheta, cMin, xCenter, C, cBest
+
+    def setup_sample(self, iterations, foundGoal, cMin, xCenter, C, cBest):
+
+        if len(self.vertex_queue) == 0 and len(self.edge_queue) == 0:
+            print("Batch: ", iterations)
+            # Using informed rrt star way of computing the samples
+            self.r = 2.0
+            if iterations != 0:
+                if foundGoal:
+                    # a better way to do this would be to make number of samples
+                    # a function of cMin
+                    m = 200
+                    self.samples = dict()
+                    self.samples[self.goalId] = self.goal
+                else:
+                    m = 100
+                cBest = self.g_scores[self.goalId]
+                self.samples.update(self.informedSample(
+                    m, cBest, cMin, xCenter, C))
+
+            # make the old vertices the new vertices
+            self.old_vertices += self.tree.vertices.keys()
+            # add the vertices to the vertex queue
+            for nid in self.tree.vertices.keys():
+                if nid not in self.vertex_queue:
+                    self.vertex_queue.append(nid)
+        return cBest
+
+    def plan(self, animation=True):
+
+        etheta, cMin, xCenter, C, cBest = self.setup_planning()
+        iterations = 0
+
+        foundGoal = False
+        # run until done
+        while (iterations < self.maxIter):
+            cBest = self.setup_sample(iterations,
+                                      foundGoal, cMin, xCenter, C, cBest)
+            # expand the best vertices until an edge is better than the vertex
+            # this is done because the vertex cost represents the lower bound
+            # on the edge cost
+            while(self.bestVertexQueueValue() <= self.bestEdgeQueueValue()):
+                self.expandVertex(self.bestInVertexQueue())
+
+            # add the best edge to the tree
+            bestEdge = self.bestInEdgeQueue()
+            self.edge_queue.remove(bestEdge)
+
+            # Check if this can improve the current solution
+            estimatedCostOfVertex = self.g_scores[bestEdge[0]] + self.computeDistanceCost(
+                bestEdge[0], bestEdge[1]) + self.computeHeuristicCost(bestEdge[1], self.goalId)
+            estimatedCostOfEdge = self.computeDistanceCost(self.startId, bestEdge[0]) + self.computeHeuristicCost(
+                bestEdge[0], bestEdge[1]) + self.computeHeuristicCost(bestEdge[1], self.goalId)
+            actualCostOfEdge = self.g_scores[bestEdge[0]] + \
+                self.computeDistanceCost(bestEdge[0], bestEdge[1])
+
+            f1 = estimatedCostOfVertex < self.g_scores[self.goalId]
+            f2 = estimatedCostOfEdge < self.g_scores[self.goalId]
+            f3 = actualCostOfEdge < self.g_scores[self.goalId]
+
+            if f1 and f2 and f3:
+                # connect this edge
+                firstCoord = self.tree.nodeIdToRealWorldCoord(
+                    bestEdge[0])
+                secondCoord = self.tree.nodeIdToRealWorldCoord(
+                    bestEdge[1])
+                path = self.connect(firstCoord, secondCoord)
+                lastEdge = self.tree.realWorldToNodeId(secondCoord)
+                if path is None or len(path) == 0:
+                    continue
+                nextCoord = path[len(path) - 1, :]
+                nextCoordPathId = self.tree.realWorldToNodeId(
+                    nextCoord)
+                bestEdge = (bestEdge[0], nextCoordPathId)
+                if(bestEdge[1] in self.tree.vertices.keys()):
+                    continue
+                else:
+                    try:
+                        del self.samples[bestEdge[1]]
+                    except(KeyError):
+                        pass
+                    eid = self.tree.addVertex(nextCoord)
+                    self.vertex_queue.append(eid)
+                if eid == self.goalId or bestEdge[0] == self.goalId or bestEdge[1] == self.goalId:
+                    print("Goal found")
+                    foundGoal = True
+
+                self.tree.addEdge(bestEdge[0], bestEdge[1])
+
+                g_score = self.computeDistanceCost(
+                    bestEdge[0], bestEdge[1])
+                self.g_scores[bestEdge[1]] = g_score + \
+                    self.g_scores[bestEdge[0]]
+                self.f_scores[bestEdge[1]] = g_score + \
+                    self.computeHeuristicCost(bestEdge[1], self.goalId)
+                self.updateGraph()
+
+                # visualize new edge
+                if animation:
+                    self.drawGraph(xCenter=xCenter, cBest=cBest,
+                                   cMin=cMin, etheta=etheta, samples=self.samples.values(),
+                                   start=firstCoord, end=secondCoord, tree=self.tree.edges)
+
+                self.remove_queue(lastEdge, bestEdge)
+
+            else:
+                print("Nothing good")
+                self.edge_queue = []
+                self.vertex_queue = []
+
+            iterations += 1
+
+        print("Finding the path")
+        return self.find_final_path()
+
+    def find_final_path(self):
+        plan = []
+        plan.append(self.goal)
+        currId = self.goalId
+        while (currId != self.startId):
+            plan.append(self.tree.nodeIdToRealWorldCoord(currId))
+            try:
+                currId = self.nodes[currId]
+            except(KeyError):
+                print("Path key error")
+                return []
+
+        plan.append(self.start)
+        plan = plan[::-1]  # reverse the plan
+
+        return plan
+
+    def remove_queue(self, lastEdge, bestEdge):
+        for edge in self.edge_queue:
+            if(edge[1] == bestEdge[1]):
+                if self.g_scores[edge[1]] + self.computeDistanceCost(edge[1], bestEdge[1]) >= self.g_scores[self.goalId]:
+                    if(lastEdge, bestEdge[1]) in self.edge_queue:
+                        self.edge_queue.remove(
+                            (lastEdge, bestEdge[1]))
+
+    def connect(self, start, end):
+        # A function which attempts to extend from a start coordinates
+        # to goal coordinates
+        steps = int(self.computeDistanceCost(self.tree.realWorldToNodeId(
+            start), self.tree.realWorldToNodeId(end)) * 10)
+        x = np.linspace(start[0], end[0], num=steps)
+        y = np.linspace(start[1], end[1], num=steps)
+        for i in range(len(x)):
+            if(self._collisionCheck(x[i], y[i])):
+                if(i == 0):
+                    return None
+                # if collision, send path until collision
+                return np.vstack((x[0:i], y[0:i])).transpose()
+
+        return np.vstack((x, y)).transpose()
+
+    def _collisionCheck(self, x, y):
+        for (ox, oy, size) in self.obstacleList:
+            dx = ox - x
+            dy = oy - y
+            d = dx * dx + dy * dy
+            if d <= size ** 2:
+                return True  # collision
+        return False
+
+    # def prune(self, c):
+
+    def computeHeuristicCost(self, start_id, goal_id):
+        # Using Manhattan distance as heuristic
+        start = np.array(self.tree.nodeIdToRealWorldCoord(start_id))
+        goal = np.array(self.tree.nodeIdToRealWorldCoord(goal_id))
+
+        return np.linalg.norm(start - goal, 2)
+
+    def computeDistanceCost(self, vid, xid):
+        # L2 norm distance
+        start = np.array(self.tree.nodeIdToRealWorldCoord(vid))
+        stop = np.array(self.tree.nodeIdToRealWorldCoord(xid))
+
+        return np.linalg.norm(stop - start, 2)
+
+    # Sample free space confined in the radius of ball R
+    def informedSample(self, m, cMax, cMin, xCenter, C):
+        samples = dict()
+        print("g_Score goal id: ", self.g_scores[self.goalId])
+        for i in range(m + 1):
+            if cMax < float('inf'):
+                r = [cMax / 2.0,
+                     math.sqrt(cMax**2 - cMin**2) / 2.0,
+                     math.sqrt(cMax**2 - cMin**2) / 2.0]
+                L = np.diag(r)
+                xBall = self.sampleUnitBall()
+                rnd = np.dot(np.dot(C, L), xBall) + xCenter
+                rnd = [rnd[(0, 0)], rnd[(1, 0)]]
+                random_id = self.tree.realWorldToNodeId(rnd)
+                samples[random_id] = rnd
+            else:
+                rnd = self.sampleFreeSpace()
+                random_id = self.tree.realWorldToNodeId(rnd)
+                samples[random_id] = rnd
+        return samples
+
+    # Sample point in a unit ball
+    def sampleUnitBall(self):
+        a = random.random()
+        b = random.random()
+
+        if b < a:
+            a, b = b, a
+
+        sample = (b * math.cos(2 * math.pi * a / b),
+                  b * math.sin(2 * math.pi * a / b))
+        return np.array([[sample[0]], [sample[1]], [0]])
+
+    def sampleFreeSpace(self):
+        rnd = [random.uniform(self.minrand, self.maxrand),
+               random.uniform(self.minrand, self.maxrand)]
+
+        return rnd
+
+    def bestVertexQueueValue(self):
+        if(len(self.vertex_queue) == 0):
+            return float('inf')
+        values = [self.g_scores[v] +
+                  self.computeHeuristicCost(v, self.goalId) for v in self.vertex_queue]
+        values.sort()
+        return values[0]
+
+    def bestEdgeQueueValue(self):
+        if(len(self.edge_queue) == 0):
+            return float('inf')
+        # return the best value in the queue by score g_tau[v] + c(v,x) + h(x)
+        values = [self.g_scores[e[0]] + self.computeDistanceCost(e[0], e[1]) +
+                  self.computeHeuristicCost(e[1], self.goalId) for e in self.edge_queue]
+        values.sort(reverse=True)
+        return values[0]
+
+    def bestInVertexQueue(self):
+        # return the best value in the vertex queue
+        v_plus_vals = [(v, self.g_scores[v] + self.computeHeuristicCost(v, self.goalId))
+                       for v in self.vertex_queue]
+        v_plus_vals = sorted(v_plus_vals, key=lambda x: x[1])
+        # print(v_plus_vals)
+        return v_plus_vals[0][0]
+
+    def bestInEdgeQueue(self):
+        e_and_values = [(e[0], e[1], self.g_scores[e[0]] + self.computeDistanceCost(
+            e[0], e[1]) + self.computeHeuristicCost(e[1], self.goalId)) for e in self.edge_queue]
+        e_and_values = sorted(e_and_values, key=lambda x: x[2])
+
+        return (e_and_values[0][0], e_and_values[0][1])
+
+    def expandVertex(self, vid):
+        self.vertex_queue.remove(vid)
+
+        # get the coordinates for given vid
+        currCoord = np.array(self.tree.nodeIdToRealWorldCoord(vid))
+
+        # get the nearest value in vertex for every one in samples where difference is
+        # less than the radius
+        neigbors = []
+        for sid, scoord in self.samples.items():
+            scoord = np.array(scoord)
+            if(np.linalg.norm(scoord - currCoord, 2) <= self.r and sid != vid):
+                neigbors.append((sid, scoord))
+
+        # add an edge to the edge queue is the path might improve the solution
+        for neighbor in neigbors:
+            sid = neighbor[0]
+            scoord = neighbor[1]
+            estimated_f_score = self.computeDistanceCost(
+                self.startId, vid) + self.computeHeuristicCost(sid, self.goalId) + self.computeDistanceCost(vid, sid)
+            if estimated_f_score < self.g_scores[self.goalId]:
+                self.edge_queue.append((vid, sid))
+
+        # add the vertex to the edge queue
+        self.add_vertex_to_edge_queue(vid, currCoord)
+
+    def add_vertex_to_edge_queue(self, vid, currCoord):
+        if vid not in self.old_vertices:
+            neigbors = []
+            for v, edges in self.tree.vertices.items():
+                if v != vid and (v, vid) not in self.edge_queue and (vid, v) not in self.edge_queue:
+                    vcoord = self.tree.nodeIdToRealWorldCoord(v)
+                    if(np.linalg.norm(vcoord - currCoord, 2) <= self.r and v != vid):
+                        neigbors.append((vid, vcoord))
+
+            for neighbor in neigbors:
+                sid = neighbor[0]
+                estimated_f_score = self.computeDistanceCost(self.startId, vid) + \
+                    self.computeDistanceCost(
+                        vid, sid) + self.computeHeuristicCost(sid, self.goalId)
+                if estimated_f_score < self.g_scores[self.goalId] and (self.g_scores[vid] + self.computeDistanceCost(vid, sid)) < self.g_scores[sid]:
+                    self.edge_queue.append((vid, sid))
+
+    def updateGraph(self):
+        closedSet = []
+        openSet = []
+        currId = self.startId
+        openSet.append(currId)
+
+        while len(openSet) != 0:
+            # get the element with lowest f_score
+            currId = min(openSet, key=lambda x: self.f_scores[x])
+
+            # remove element from open set
+            openSet.remove(currId)
+
+            # Check if we're at the goal
+            if(currId == self.goalId):
+                self.nodes[self.goalId]
+                break
+
+            if(currId not in closedSet):
+                closedSet.append(currId)
+
+            # find a non visited successor to the current node
+            successors = self.tree.vertices[currId]
+            for succesor in successors:
+                if(succesor in closedSet):
+                    continue
+                else:
+                    # claculate tentative g score
+                    g_score = self.g_scores[currId] + \
+                        self.computeDistanceCost(currId, succesor)
+                    if succesor not in openSet:
+                        # add the successor to open set
+                        openSet.append(succesor)
+                    elif g_score >= self.g_scores[succesor]:
+                        continue
+
+                    # update g and f scores
+                    self.g_scores[succesor] = g_score
+                    self.f_scores[succesor] = g_score + \
+                        self.computeHeuristicCost(succesor, self.goalId)
+
+                    # store the parent and child
+                    self.nodes[succesor] = currId
+
+    def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None,
+                  samples=None, start=None, end=None, tree=None):
+        print("Plotting Graph")
+        plt.clf()
+        for rnd in samples:
+            if rnd is not None:
+                plt.plot(rnd[0], rnd[1], "^k")
+                if cBest != float('inf'):
+                    self.plot_ellipse(xCenter, cBest, cMin, etheta)
+
+        if start is not None and end is not None:
+            plt.plot([start[0], start[1]], [end[0], end[1]], "-g")
+
+        for (ox, oy, size) in self.obstacleList:
+            plt.plot(ox, oy, "ok", ms=30 * size)
+
+        plt.plot(self.start[0], self.start[1], "xr")
+        plt.plot(self.goal[0], self.goal[1], "xr")
+        plt.axis([-2, 15, -2, 15])
+        plt.grid(True)
+        plt.pause(0.01)
+
+    def plot_ellipse(self, xCenter, cBest, cMin, etheta):
+
+        a = math.sqrt(cBest**2 - cMin**2) / 2.0
+        b = cBest / 2.0
+        angle = math.pi / 2.0 - etheta
+        cx = xCenter[0]
+        cy = xCenter[1]
+
+        t = np.arange(0, 2 * math.pi + 0.1, 0.1)
+        x = [a * math.cos(it) for it in t]
+        y = [b * math.sin(it) for it in t]
+        R = np.matrix([[math.cos(angle), math.sin(angle)],
+                       [-math.sin(angle), math.cos(angle)]])
+        fx = R * np.matrix([x, y])
+        px = np.array(fx[0, :] + cx).flatten()
+        py = np.array(fx[1, :] + cy).flatten()
+        plt.plot(cx, cy, "xc")
+        plt.plot(px, py, "--c")
+
+
+def main():
+    print("Starting Batch Informed Trees Star planning")
+    obstacleList = [
+        (5, 5, 0.5),
+        (9, 6, 1),
+        (7, 5, 1),
+        (1, 5, 1),
+        (3, 6, 1),
+        (7, 9, 1)
+    ]
+
+    bitStar = BITStar(start=[-1, 0], goal=[3, 8], obstacleList=obstacleList,
+                      randArea=[-2, 15])
+    path = bitStar.plan(animation=show_animation)
+    print("Done")
+
+    if show_animation:
+        plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
+        plt.grid(True)
+        plt.pause(0.05)
+        plt.show()
+
+
+if __name__ == '__main__':
+    main()

PathPlanning/BezierPath/bezier_path.py

@@ -1,109 +1,203 @@
 """
 
-Path Planning with 4 point Beizer curve
+Path Planning with Bezier curve.
 
 author: Atsushi Sakai(@Atsushi_twi)
 
 """
+from __future__ import division, print_function
 
 import scipy.special
 import numpy as np
 import matplotlib.pyplot as plt
-import math
 
 show_animation = True
 
 
-def calc_4point_bezier_path(sx, sy, syaw, ex, ey, eyaw, offset):
-    D = math.sqrt((sx - ex)**2 + (sy - ey)**2) / offset
-    cp = np.array(
+def calc_4points_bezier_path(sx, sy, syaw, ex, ey, eyaw, offset):
+    """
+    Compute control points and path given start and end position.
+
+    :param sx: (float) x-coordinate of the starting point
+    :param sy: (float) y-coordinate of the starting point
+    :param syaw: (float) yaw angle at start
+    :param ex: (float) x-coordinate of the ending point
+    :param ey: (float) y-coordinate of the ending point
+    :param eyaw: (float) yaw angle at the end
+    :param offset: (float)
+    :return: (numpy array, numpy array)
+    """
+    dist = np.sqrt((sx - ex) ** 2 + (sy - ey) ** 2) / offset
+    control_points = np.array(
         [[sx, sy],
-         [sx + D * math.cos(syaw), sy + D * math.sin(syaw)],
-         [ex - D * math.cos(eyaw), ey - D * math.sin(eyaw)],
+         [sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)],
+         [ex - dist * np.cos(eyaw), ey - dist * np.sin(eyaw)],
          [ex, ey]])
 
+    path = calc_bezier_path(control_points, n_points=100)
+
+    return path, control_points
+
+
+def calc_bezier_path(control_points, n_points=100):
+    """
+    Compute bezier path (trajectory) given control points.
+
+    :param control_points: (numpy array)
+    :param n_points: (int) number of points in the trajectory
+    :return: (numpy array)
+    """
     traj = []
-    for t in np.linspace(0, 1, 100):
-        traj.append(bezier(3, t, cp))
-    P = np.array(traj)
+    for t in np.linspace(0, 1, n_points):
+        traj.append(bezier(t, control_points))
 
-    return P, cp
+    return np.array(traj)
 
 
-def bernstein(n, i, t):
-    return scipy.special.comb(n, i) * t**i * (1 - t)**(n - i)
+def bernstein_poly(n, i, t):
+    """
+    Bernstein polynom.
+
+    :param n: (int) polynom degree
+    :param i: (int)
+    :param t: (float)
+    :return: (float)
+    """
+    return scipy.special.comb(n, i) * t ** i * (1 - t) ** (n - i)
 
 
-def bezier(n, t, q):
-    p = np.zeros(2)
-    for i in range(n + 1):
-        p += bernstein(n, i, t) * q[i]
-    return p
+def bezier(t, control_points):
+    """
+    Return one point on the bezier curve.
+
+    :param t: (float) number in [0, 1]
+    :param control_points: (numpy array)
+    :return: (numpy array) Coordinates of the point
+    """
+    n = len(control_points) - 1
+    return np.sum([bernstein_poly(n, i, t) * control_points[i] for i in range(n + 1)], axis=0)
+
+
+def bezier_derivatives_control_points(control_points, n_derivatives):
+    """
+    Compute control points of the successive derivatives of a given bezier curve.
+
+    A derivative of a bezier curve is a bezier curve.
+    See https://pomax.github.io/bezierinfo/#derivatives
+    for detailed explanations
+
+    :param control_points: (numpy array)
+    :param n_derivatives: (int)
+    e.g., n_derivatives=2 -> compute control points for first and second derivatives
+    :return: ([numpy array])
+    """
+    w = {0: control_points}
+    for i in range(n_derivatives):
+        n = len(w[i])
+        w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j]) for j in range(n - 1)])
+    return w
+
+
+def curvature(dx, dy, ddx, ddy):
+    """
+    Compute curvature at one point given first and second derivatives.
+
+    :param dx: (float) First derivative along x axis
+    :param dy: (float)
+    :param ddx: (float) Second derivative along x axis
+    :param ddy: (float)
+    :return: (float)
+    """
+    return (dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** (3 / 2)
 
 
 def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
-    u"""
-    Plot arrow
-    """
-
+    """Plot arrow."""
     if not isinstance(x, float):
         for (ix, iy, iyaw) in zip(x, y, yaw):
             plot_arrow(ix, iy, iyaw)
     else:
-        plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
+        plt.arrow(x, y, length * np.cos(yaw), length * np.sin(yaw),
                   fc=fc, ec=ec, head_width=width, head_length=width)
         plt.plot(x, y)
 
 
 def main():
+    """Plot an example bezier curve."""
     start_x = 10.0  # [m]
     start_y = 1.0  # [m]
-    start_yaw = math.radians(180.0)  # [rad]
+    start_yaw = np.radians(180.0)  # [rad]
 
     end_x = -0.0  # [m]
     end_y = -3.0  # [m]
-    end_yaw = math.radians(-45.0)  # [rad]
+    end_yaw = np.radians(-45.0)  # [rad]
     offset = 3.0
 
-    P, cp = calc_4point_bezier_path(
+    path, control_points = calc_4points_bezier_path(
         start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset)
 
-    assert P.T[0][0] == start_x, "path is invalid"
-    assert P.T[1][0] == start_y, "path is invalid"
-    assert P.T[0][-1] == end_x, "path is invalid"
-    assert P.T[1][-1] == end_y, "path is invalid"
+    # Note: alternatively, instead of specifying start and end position
+    # you can directly define n control points and compute the path:
+    # control_points = np.array([[5., 1.], [-2.78, 1.], [-11.5, -4.5], [-6., -8.]])
+    # path = calc_bezier_path(control_points, n_points=100)
+
+    # Display the tangent, normal and radius of cruvature at a given point
+    t = 0.86  # Number in [0, 1]
+    x_target, y_target = bezier(t, control_points)
+    derivatives_cp = bezier_derivatives_control_points(control_points, 2)
+    point = bezier(t, control_points)
+    dt = bezier(t, derivatives_cp[1])
+    ddt = bezier(t, derivatives_cp[2])
+    # Radius of curvature
+    radius = 1 / curvature(dt[0], dt[1], ddt[0], ddt[1])
+    # Normalize derivative
+    dt /= np.linalg.norm(dt, 2)
+    tangent = np.array([point, point + dt])
+    normal = np.array([point, point + [- dt[1], dt[0]]])
+    curvature_center = point + np.array([- dt[1], dt[0]]) * radius
+    circle = plt.Circle(tuple(curvature_center), radius, color=(0, 0.8, 0.8), fill=False, linewidth=1)
+
+    assert path.T[0][0] == start_x, "path is invalid"
+    assert path.T[1][0] == start_y, "path is invalid"
+    assert path.T[0][-1] == end_x, "path is invalid"
+    assert path.T[1][-1] == end_y, "path is invalid"
 
     if show_animation:
-        plt.plot(P.T[0], P.T[1], label="Bezier Path")
-        plt.plot(cp.T[0], cp.T[1], '--o', label="Control Points")
+        fig, ax = plt.subplots()
+        ax.plot(path.T[0], path.T[1], label="Bezier Path")
+        ax.plot(control_points.T[0], control_points.T[1], '--o', label="Control Points")
+        ax.plot(x_target, y_target)
+        ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent")
+        ax.plot(normal[:, 0], normal[:, 1], label="Normal")
+        ax.add_artist(circle)
         plot_arrow(start_x, start_y, start_yaw)
         plot_arrow(end_x, end_y, end_yaw)
-        plt.legend()
-        plt.axis("equal")
-        plt.grid(True)
+        ax.legend()
+        ax.axis("equal")
+        ax.grid(True)
         plt.show()
 
 
 def main2():
+    """Show the effect of the offset."""
     start_x = 10.0  # [m]
     start_y = 1.0  # [m]
-    start_yaw = math.radians(180.0)  # [rad]
+    start_yaw = np.radians(180.0)  # [rad]
 
     end_x = -0.0  # [m]
     end_y = -3.0  # [m]
-    end_yaw = math.radians(-45.0)  # [rad]
-    offset = 3.0
+    end_yaw = np.radians(-45.0)  # [rad]
 
     for offset in np.arange(1.0, 5.0, 1.0):
-        P, cp = calc_4point_bezier_path(
+        path, control_points = calc_4points_bezier_path(
             start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset)
-        assert P.T[0][0] == start_x, "path is invalid"
-        assert P.T[1][0] == start_y, "path is invalid"
-        assert P.T[0][-1] == end_x, "path is invalid"
-        assert P.T[1][-1] == end_y, "path is invalid"
+        assert path.T[0][0] == start_x, "path is invalid"
+        assert path.T[1][0] == start_y, "path is invalid"
+        assert path.T[0][-1] == end_x, "path is invalid"
+        assert path.T[1][-1] == end_y, "path is invalid"
 
         if show_animation:
-            plt.plot(P.T[0], P.T[1], label="Offset=" + str(offset))
+            plt.plot(path.T[0], path.T[1], label="Offset=" + str(offset))
 
     if show_animation:
         plot_arrow(start_x, start_y, start_yaw)

PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

@@ -12,11 +12,12 @@ import random
 import math
 import copy
 import numpy as np
-import reeds_shepp_path_planning
 import pure_pursuit
-import unicycle_model
 import matplotlib.pyplot as plt
 
+import reeds_shepp_path_planning
+import unicycle_model
+
 show_animation = True
 
 
@@ -220,14 +221,10 @@ class RRT():
 
         return newNode
 
+
     def pi_2_pi(self, angle):
-        while(angle > math.pi):
-            angle = angle - 2.0 * math.pi
+        return (angle + math.pi) % (2*math.pi) - math.pi
 
-        while(angle < -math.pi):
-            angle = angle + 2.0 * math.pi
-
-        return angle
 
     def steer(self, rnd, nind):
         #  print(rnd)
@@ -337,18 +334,14 @@ class RRT():
                 self.nodeList[i] = tNode
 
     def DrawGraph(self, rnd=None):
-        u"""
+        """
         Draw Graph
         """
-        #  plt.clf()
         if rnd is not None:
             plt.plot(rnd.x, rnd.y, "^k")
         for node in self.nodeList:
             if node.parent is not None:
                 plt.plot(node.path_x, node.path_y, "-g")
-                pass
-                #  plt.plot([node.x, self.nodeList[node.parent].x], [
-                #  node.y, self.nodeList[node.parent].y], "-g")
 
         for (ox, oy, size) in self.obstacleList:
             plt.plot(ox, oy, "ok", ms=30 * size)
@@ -362,9 +355,6 @@ class RRT():
         plt.grid(True)
         plt.pause(0.01)
 
-        #  plt.show()
-        #  input()
-
     def GetNearestListIndex(self, nodeList, rnd):
         dlist = [(node.x - rnd.x) ** 2 +
                  (node.y - rnd.y) ** 2 +
@@ -399,7 +389,7 @@ class RRT():
 
 
 class Node():
-    u"""
+    """
     RRT Node
     """
 
@@ -417,14 +407,6 @@ class Node():
 def main():
     print("Start rrt start planning")
     # ====Search Path with RRT====
-    obstacleList = [
-        (5, 5, 1),
-        (3, 6, 2),
-        (3, 8, 2),
-        (3, 10, 2),
-        (7, 5, 2),
-        (9, 5, 2)
-    ]  # [x,y,size(radius)]
     obstacleList = [
         (5, 5, 1),
         (4, 6, 1),

PathPlanning/ClosedLoopRRTStar/unicycle_model.py

@@ -1,15 +1,15 @@
-#! /usr/bin/python
-# -*- coding: utf-8 -*-
 """
 
+Unicycle model class
 
 author Atsushi Sakai
+
 """
 
 import math
 
 dt = 0.05  # [s]
-L = 2.9  # [m]
+L = 0.9  # [m]
 steer_max = math.radians(40.0)
 curvature_max = math.tan(steer_max) / L
 curvature_max = 1.0 / curvature_max + 1.0
@@ -38,13 +38,7 @@ def update(state, a, delta):
 
 
 def pi_2_pi(angle):
-    while(angle > math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while(angle < -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
+    return (angle + math.pi) % (2*math.pi) - math.pi
 
 
 if __name__ == '__main__':

PathPlanning/CubicSpline/cubic_spline_planner.py

@@ -1,7 +1,7 @@
 """
-cubic spline planner
+Cubic spline planner
 
-Author: Atsushi Sakai
+Author: Atsushi Sakai(@Atsushi_twi)
 
 """
 import math
@@ -10,7 +10,7 @@ import bisect
 
 
 class Spline:
-    u"""
+    """
     Cubic Spline class
     """
 
@@ -40,7 +40,7 @@ class Spline:
             self.b.append(tb)
 
     def calc(self, t):
-        u"""
+        """
         Calc position
 
         if t is outside of the input x, return None
@@ -60,7 +60,7 @@ class Spline:
         return result
 
     def calcd(self, t):
-        u"""
+        """
         Calc first derivative
 
         if t is outside of the input x, return None
@@ -77,7 +77,7 @@ class Spline:
         return result
 
     def calcdd(self, t):
-        u"""
+        """
         Calc second derivative
         """
 
@@ -92,13 +92,13 @@ class Spline:
         return result
 
     def __search_index(self, x):
-        u"""
+        """
         search data segment index
         """
         return bisect.bisect(self.x, x) - 1
 
     def __calc_A(self, h):
-        u"""
+        """
         calc matrix A for spline coefficient c
         """
         A = np.zeros((self.nx, self.nx))
@@ -116,19 +116,18 @@ class Spline:
         return A
 
     def __calc_B(self, h):
-        u"""
+        """
         calc matrix B for spline coefficient c
         """
         B = np.zeros(self.nx)
         for i in range(self.nx - 2):
             B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \
                 h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i]
-        #  print(B)
         return B
 
 
 class Spline2D:
-    u"""
+    """
     2D Cubic Spline class
 
     """
@@ -148,7 +147,7 @@ class Spline2D:
         return s
 
     def calc_position(self, s):
-        u"""
+        """
         calc position
         """
         x = self.sx.calc(s)
@@ -157,18 +156,18 @@ class Spline2D:
         return x, y
 
     def calc_curvature(self, s):
-        u"""
+        """
         calc curvature
         """
         dx = self.sx.calcd(s)
         ddx = self.sx.calcdd(s)
         dy = self.sy.calcd(s)
         ddy = self.sy.calcdd(s)
-        k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
+        k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2))
         return k
 
     def calc_yaw(self, s):
-        u"""
+        """
         calc yaw
         """
         dx = self.sx.calcd(s)
@@ -197,9 +196,10 @@ def main():
     import matplotlib.pyplot as plt
     x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
     y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]
+    ds = 0.1  # [m] distance of each intepolated points
 
     sp = Spline2D(x, y)
-    s = np.arange(0, sp.s[-1], 0.1)
+    s = np.arange(0, sp.s[-1], ds)
 
     rx, ry, ryaw, rk = [], [], [], []
     for i_s in s:

PathPlanning/Dijkstra/dijkstra.py

@@ -85,12 +85,12 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, reso, rr):
             else:
                 openset[n_id] = node
 
-    rx, ry = calc_fianl_path(ngoal, closedset, reso)
+    rx, ry = calc_final_path(ngoal, closedset, reso)
 
     return rx, ry
 
 
-def calc_fianl_path(ngoal, closedset, reso):
+def calc_final_path(ngoal, closedset, reso):
     # generate final course
     rx, ry = [ngoal.x * reso], [ngoal.y * reso]
     pind = ngoal.pind

PathPlanning/DubinsPath/dubins_path_planning.py

@@ -17,13 +17,7 @@ def mod2pi(theta):
 
 
 def pi_2_pi(angle):
-    while(angle >= math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while(angle <= -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
+    return (angle + math.pi) % (2*math.pi) - math.pi
 
 
 def LSL(alpha, beta, d):

PathPlanning/Eta3SplinePath/eta3_spline_path.py

@@ -0,0 +1,308 @@
+"""
+
+\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/)
+
+"""
+
+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
+
+show_animation = True
+
+
+class eta3_path(object):
+    """
+    eta3_path
+
+    input
+        segments: list of `eta3_path_segment` instances definining a continuous path
+    """
+
+    def __init__(self, segments):
+        # ensure input has the correct form
+        assert(isinstance(segments, list) and isinstance(
+            segments[0], eta3_path_segment))
+        # 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))
+        self.segments = segments
+    """
+    eta3_path::calc_path_point
+
+    input
+        normalized interpolation point along path object, 0 <= u <= len(self.segments)
+    returns
+        2d (x,y) position vector
+    """
+
+    def calc_path_point(self, u):
+        assert(u >= 0 and u <= len(self.segments))
+        if np.isclose(u, len(self.segments)):
+            segment_idx = len(self.segments) - 1
+            u = 1.
+        else:
+            segment_idx = int(np.floor(u))
+            u -= segment_idx
+        return self.segments[segment_idx].calc_point(u)
+
+
+class eta3_path_segment(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.
+
+    input
+        start_pose - starting pose array  (x, y, \theta)
+        end_pose - ending pose array (x, y, \theta)
+        eta - shaping parameters, default=None
+        kappa - curvature parameters, default=None
+    """
+
+    def __init__(self, start_pose, end_pose, eta=None, kappa=None):
+        # make sure inputs are of the correct size
+        assert(len(start_pose) == 3 and len(start_pose) == len(end_pose))
+        self.start_pose = start_pose
+        self.end_pose = end_pose
+        # if no eta is passed, initialize it to array of zeros
+        if not eta:
+            eta = np.zeros((6,))
+        else:
+            # make sure that eta has correct size
+            assert(len(eta) == 6)
+        # if no kappa is passed, initialize to array of zeros
+        if not kappa:
+            kappa = np.zeros((4,))
+        else:
+            assert(len(kappa) == 4)
+        # set up angle cosines and sines for simpler computations below
+        ca = np.cos(start_pose[2])
+        sa = np.sin(start_pose[2])
+        cb = np.cos(end_pose[2])
+        sb = np.sin(end_pose[2])
+        # 2 dimensions (x,y) x 8 coefficients per dimension
+        self.coeffs = np.empty((2, 8))
+        # constant terms (u^0)
+        self.coeffs[0, 0] = start_pose[0]
+        self.coeffs[1, 0] = start_pose[1]
+        # linear (u^1)
+        self.coeffs[0, 1] = eta[0] * ca
+        self.coeffs[1, 1] = eta[0] * sa
+        # quadratic (u^2)
+        self.coeffs[0, 2] = 1. / 2 * eta[2] * \
+            ca - 1. / 2 * eta[0]**2 * kappa[0] * sa
+        self.coeffs[1, 2] = 1. / 2 * eta[2] * \
+            sa + 1. / 2 * eta[0]**2 * kappa[0] * ca
+        # cubic (u^3)
+        self.coeffs[0, 3] = 1. / 6 * eta[4] * ca - 1. / 6 * \
+            (eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * sa
+        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
+        # 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
+        # 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
+        # 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
+        
+        s_dot = lambda u : 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])))
+        self.segment_length = quad(lambda u: s_dot(u), 0, 1)[0]
+
+    """
+    eta3_path_segment::calc_point
+
+    input
+        u - parametric representation of a point along the segment, 0 <= u <= 1
+    returns
+        (x,y) of point along the segment
+    """
+    def calc_point(self, u):
+        assert(u >= 0 and u <= 1)
+        return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7]))
+
+
+def test1():
+
+    for i in range(10):
+        path_segments = []
+        # segment 1: lane-change curve
+        start_pose = [0, 0, 0]
+        end_pose = [4, 3.0, 0]
+        # NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative
+        kappa = [0, 0, 0, 0]
+        eta = [i, i, 0, 0, 0, 0]
+        path_segments.append(eta3_path_segment(
+            start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
+
+        path = eta3_path(path_segments)
+
+        # interpolate at several points along the path
+        ui = np.linspace(0, len(path_segments), 1001)
+        pos = np.empty((2, ui.size))
+        for i, u in enumerate(ui):
+            pos[:, i] = path.calc_path_point(u)
+
+        if show_animation:
+            # plot the path
+            plt.plot(pos[0, :], pos[1, :])
+            plt.pause(1.0)
+
+    if show_animation:
+        plt.close("all")
+
+
+def test2():
+
+    for i in range(10):
+        path_segments = []
+        # segment 1: lane-change curve
+        start_pose = [0, 0, 0]
+        end_pose = [4, 3.0, 0]
+        # NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative
+        kappa = [0, 0, 0, 0]
+        eta = [0, 0, (i - 5) * 20, (5 - i) * 20, 0, 0]
+        path_segments.append(eta3_path_segment(
+            start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
+
+        path = eta3_path(path_segments)
+
+        # interpolate at several points along the path
+        ui = np.linspace(0, len(path_segments), 1001)
+        pos = np.empty((2, ui.size))
+        for i, u in enumerate(ui):
+            pos[:, i] = path.calc_path_point(u)
+
+        if show_animation:
+            # plot the path
+            plt.plot(pos[0, :], pos[1, :])
+            plt.pause(1.0)
+
+    if show_animation:
+        plt.close("all")
+
+
+def test3():
+    path_segments = []
+
+    # segment 1: lane-change curve
+    start_pose = [0, 0, 0]
+    end_pose = [4, 1.5, 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]
+    path_segments.append(eta3_path_segment(
+        start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
+
+    # segment 2: line segment
+    start_pose = [4, 1.5, 0]
+    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(
+        start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
+
+    # segment 3: cubic spiral
+    start_pose = [5.5, 1.5, 0]
+    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(
+        start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
+
+    # segment 4: generic twirl arc
+    start_pose = [7.4377, 1.8235, 0.6667]
+    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(
+        start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
+
+    # segment 5: circular arc
+    start_pose = [7.8, 4.3, 1.8]
+    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(
+        start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
+
+    # construct the whole path
+    path = eta3_path(path_segments)
+
+    # interpolate at several points along the path
+    ui = np.linspace(0, len(path_segments), 1001)
+    pos = np.empty((2, ui.size))
+    for i, u in enumerate(ui):
+        pos[:, i] = path.calc_path_point(u)
+
+    # plot the path
+
+    if show_animation:
+        plt.figure('Path from Reference')
+        plt.plot(pos[0, :], pos[1, :])
+        plt.xlabel('x')
+        plt.ylabel('y')
+        plt.title('Path')
+        plt.pause(1.0)
+
+        plt.show()
+
+
+def main():
+    """
+    recreate path from reference (see Table 1)
+    """
+    test1()
+    test2()
+    test3()
+
+
+if __name__ == '__main__':
+    main()

PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py

@@ -42,11 +42,11 @@ KLON = 1.0
 show_animation = True
 
 
-class quinic_polynomial:
+class quintic_polynomial:
 
     def __init__(self, xs, vxs, axs, xe, vxe, axe, T):
 
-        # calc coefficient of quinic polynomial
+        # calc coefficient of quintic polynomial
         self.xs = xs
         self.vxs = vxs
         self.axs = axs
@@ -87,12 +87,17 @@ class quinic_polynomial:
 
         return xt
 
+    def calc_third_derivative(self, t):
+        xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t**2
+
+        return xt
+
 
 class quartic_polynomial:
 
     def __init__(self, xs, vxs, axs, vxe, axe, T):
 
-        # calc coefficient of quinic polynomial
+        # calc coefficient of quintic polynomial
         self.xs = xs
         self.vxs = vxs
         self.axs = axs
@@ -129,6 +134,11 @@ class quartic_polynomial:
 
         return xt
 
+    def calc_third_derivative(self, t):
+        xt = 6 * self.a3 + 24 * self.a4 * t
+
+        return xt
+
 
 class Frenet_path:
 
@@ -137,9 +147,11 @@ class Frenet_path:
         self.d = []
         self.d_d = []
         self.d_dd = []
+        self.d_ddd = []
         self.s = []
         self.s_d = []
         self.s_dd = []
+        self.s_ddd = []
         self.cd = 0.0
         self.cv = 0.0
         self.cf = 0.0
@@ -155,17 +167,22 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
 
     frenet_paths = []
 
+    # generate path to each offset goal
     for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
+
+        # Lateral motion planning
         for Ti in np.arange(MINT, MAXT, DT):
             fp = Frenet_path()
 
-            lat_qp = quinic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
+            lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
 
             fp.t = [t for t in np.arange(0.0, Ti, DT)]
             fp.d = [lat_qp.calc_point(t) for t in fp.t]
             fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
             fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
+            fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
 
+            # Loongitudinal motion planning (Velocity keeping)
             for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
                 tfp = copy.deepcopy(fp)
                 lon_qp = quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
@@ -173,10 +190,16 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
                 tfp.s = [lon_qp.calc_point(t) for t in fp.t]
                 tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
                 tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
+                tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
 
-                tfp.cd = KJ * sum(tfp.d_dd) + KT * Ti + KD * tfp.d[-1]**2
-                tfp.cv = KJ * sum(tfp.s_dd) + KT * Ti + KD * \
-                    (TARGET_SPEED - tfp.s_d[-1])**2
+                Jp = sum(np.power(tfp.d_ddd, 2))  # square of jerk
+                Js = sum(np.power(tfp.s_ddd, 2))  # square of jerk
+
+                # square of diff from target speed
+                ds = (TARGET_SPEED - tfp.s_d[-1])**2
+
+                tfp.cd = KJ * Jp + KT * Ti + KD * tfp.d[-1]**2
+                tfp.cv = KJ * Js + KT * Ti + KD * ds
                 tfp.cf = KLAT * tfp.cd + KLON * tfp.cv
 
                 frenet_paths.append(tfp)

PathPlanning/HybridAStar/hybrid_a_star.py

@@ -0,0 +1,222 @@
+"""
+
+Hybrid A* path planning
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+"""
+
+import sys
+sys.path.append("../ReedsSheppPath/")
+
+import math
+import numpy as np
+import scipy.spatial
+import matplotlib.pyplot as plt
+import reeds_shepp_path_planning as rs
+import heapq
+
+EXTEND_AREA = 5.0  # [m]
+H_COST = 1.0
+
+show_animation = True
+
+
+class Node:
+
+    def __init__(self, xind, yind, yawind, direction, x, y, yaw, directions, steer, cost, pind):
+        # store kd-tree
+        self.xind = xind
+        self.yind = yind
+        self.yawind = yawind
+        self.direction = direction
+        self.xlist = x
+        self.ylist = y
+        self.yawlist = yaw
+        self.directionlist = directions
+        self.steer = steer
+        self.cost = cost
+        self.pind = pind
+
+
+class KDTree:
+    """
+    Nearest neighbor search class with KDTree
+    """
+
+    def __init__(self, data):
+        # store kd-tree
+        self.tree = scipy.spatial.cKDTree(data)
+
+    def search(self, inp, k=1):
+        """
+        Search NN
+        inp: input data, single frame or multi frame
+        """
+
+        if len(inp.shape) >= 2:  # multi input
+            index = []
+            dist = []
+
+            for i in inp.T:
+                idist, iindex = self.tree.query(i, k=k)
+                index.append(iindex)
+                dist.append(idist)
+
+            return index, dist
+        else:
+            dist, index = self.tree.query(inp, k=k)
+            return index, dist
+
+    def search_in_distance(self, inp, r):
+        """
+        find points with in a distance r
+        """
+
+        index = self.tree.query_ball_point(inp, r)
+        return index
+
+
+class Config:
+
+    def __init__(self, ox, oy, xyreso, yawreso):
+        min_x_m = min(ox) - EXTEND_AREA
+        min_y_m = min(oy) - EXTEND_AREA
+        max_x_m = max(ox) + EXTEND_AREA
+        max_y_m = max(oy) + EXTEND_AREA
+
+        ox.append(min_x_m)
+        oy.append(min_y_m)
+        ox.append(max_x_m)
+        oy.append(max_y_m)
+
+        self.minx = int(min_x_m / xyreso)
+        self.miny = int(min_y_m / xyreso)
+        self.maxx = int(max_x_m / xyreso)
+        self.maxy = int(max_y_m / xyreso)
+
+        self.xw = int(self.maxx - self.minx)
+        self.yw = int(self.maxy - self.miny)
+
+        self.minyaw = int(- math.pi / yawreso) - 1
+        self.maxyaw = int(math.pi / yawreso)
+        self.yaww = int(self.maxyaw - self.minyaw)
+
+
+def analytic_expantion(current, ngoal, c, ox, oy, kdtree):
+
+    return False, None  # no update
+
+
+def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
+    """
+    start
+    goal
+    ox: x position list of Obstacles [m]
+    oy: y position list of Obstacles [m]
+    xyreso: grid resolution [m]
+    yawreso: yaw angle resolution [rad]
+    """
+
+    start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2])
+    tox, toy = ox[:], oy[:]
+
+    obkdtree = KDTree(np.vstack((tox, toy)).T)
+
+    c = Config(tox, toy, xyreso, yawreso)
+
+    nstart = Node(int(start[0] / xyreso), int(start[1] / xyreso), int(start[2] / yawreso),
+                  True, [start[0]], [start[1]], [start[2]], [True], 0.0, 0.0, -1)
+    ngoal = Node(int(goal[0] / xyreso), int(goal[1] / xyreso), int(goal[2] / yawreso),
+                 True, [goal[0]], [goal[1]], [goal[2]], [True], 0.0, 0.0, -1)
+
+    openList, closedList = {}, {}
+    h = []
+    #  goalqueue = queue.PriorityQueue()
+    pq = []
+    openList[calc_index(nstart, c)] = nstart
+    heapq.heappush(pq, (calc_index(nstart, c), calc_cost(nstart, h, ngoal, c)))
+
+    while True:
+        if not openList:
+            print("Error: Cannot find path, No open set")
+            return [], [], []
+
+        c_id, cost = heapq.heappop(pq)
+        current = openList.pop(c_id)
+        closedList[c_id] = current
+
+        isupdated, fpath = analytic_expantion(
+            current, ngoal, c, ox, oy, obkdtree)
+
+        #  print(current)
+
+    rx, ry, ryaw = [], [], []
+
+    return rx, ry, ryaw
+
+
+def calc_cost(n, h, ngoal, c):
+
+    hcost = 1.0
+
+    return (n.cost + H_COST * hcost)
+
+
+def calc_index(node, c):
+    ind = (node.yawind - c.minyaw) * c.xw * c.yw + \
+        (node.yind - c.miny) * c.xw + (node.xind - c.minx)
+
+    if ind <= 0:
+        print("Error(calc_index):", ind)
+
+    return ind
+
+
+def main():
+    print("Start Hybrid A* planning")
+
+    ox, oy = [], []
+
+    for i in range(60):
+        ox.append(i)
+        oy.append(0.0)
+    for i in range(60):
+        ox.append(60.0)
+        oy.append(i)
+    for i in range(61):
+        ox.append(i)
+        oy.append(60.0)
+    for i in range(61):
+        ox.append(0.0)
+        oy.append(i)
+    for i in range(40):
+        ox.append(20.0)
+        oy.append(i)
+    for i in range(40):
+        ox.append(40.0)
+        oy.append(60.0 - i)
+
+    # Set Initial parameters
+    start = [10.0, 10.0, math.radians(90.0)]
+    goal = [50.0, 50.0, math.radians(-90.0)]
+
+    xyreso = 2.0
+    yawreso = math.radians(15.0)
+
+    rx, ry, ryaw = hybrid_a_star_planning(
+        start, goal, ox, oy, xyreso, yawreso)
+
+    plt.plot(ox, oy, ".k")
+    rs.plot_arrow(start[0], start[1], start[2])
+    rs.plot_arrow(goal[0], goal[1], goal[2])
+
+    plt.grid(True)
+    plt.axis("equal")
+    plt.show()
+
+    print(__file__ + " start!!")
+
+
+if __name__ == '__main__':
+    main()

PathPlanning/InformedRRTStar/informed_rrt_star.py

@@ -0,0 +1,335 @@
+"""
+Informed RRT* path planning
+
+author: Karan Chawla
+        Atsushi Sakai(@Atsushi_twi)
+
+Reference: Informed RRT*: Optimal Sampling-based Path Planning Focused via
+Direct Sampling of an Admissible Ellipsoidal Heuristichttps://arxiv.org/pdf/1404.2334.pdf
+
+"""
+
+
+import random
+import numpy as np
+import math
+import copy
+import matplotlib.pyplot as plt
+
+show_animation = True
+
+
+class InformedRRTStar():
+
+    def __init__(self, start, goal,
+                 obstacleList, randArea,
+                 expandDis=0.5, goalSampleRate=10, maxIter=200):
+
+        self.start = Node(start[0], start[1])
+        self.goal = Node(goal[0], goal[1])
+        self.minrand = randArea[0]
+        self.maxrand = randArea[1]
+        self.expandDis = expandDis
+        self.goalSampleRate = goalSampleRate
+        self.maxIter = maxIter
+        self.obstacleList = obstacleList
+
+    def InformedRRTStarSearch(self, animation=True):
+
+        self.nodeList = [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
+
+        # Computing the sampling space
+        cMin = math.sqrt(pow(self.start.x - self.goal.x, 2) +
+                         pow(self.start.y - self.goal.y, 2))
+        xCenter = np.matrix([[(self.start.x + self.goal.x) / 2.0],
+                             [(self.start.y + self.goal.y) / 2.0], [0]])
+        a1 = np.matrix([[(self.goal.x - self.start.x) / cMin],
+                        [(self.goal.y - self.start.y) / cMin], [0]])
+        etheta = math.atan2(a1[1], a1[0])
+        # first column of idenity matrix transposed
+        id1_t = np.matrix([1.0, 0.0, 0.0])
+        M = np.dot(a1, id1_t)
+        U, S, Vh = np.linalg.svd(M, 1, 1)
+        C = np.dot(np.dot(U, np.diag(
+            [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh)
+
+        for i in range(self.maxIter):
+            # Sample space is defined by cBest
+            # cMin is the minimum distance between the start point and the goal
+            # xCenter is the midpoint between the start and the goal
+            # cBest changes when a new path is found
+
+            rnd = self.informed_sample(cBest, cMin, xCenter, C)
+            nind = self.getNearestListIndex(self.nodeList, rnd)
+            nearestNode = self.nodeList[nind]
+            # steer
+            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
+            newNode = self.getNewNode(theta, nind, nearestNode)
+            d = self.lineCost(nearestNode, newNode)
+
+            isCollision = self.__CollisionCheck(newNode, self.obstacleList)
+            isCollisionEx = self.check_collision_extend(nearestNode, theta, d)
+
+            if isCollision and isCollisionEx:
+                nearInds = self.findNearNodes(newNode)
+                newNode = self.chooseParent(newNode, nearInds)
+
+                self.nodeList.append(newNode)
+                self.rewire(newNode, nearInds)
+
+                if self.isNearGoal(newNode):
+                    solutionSet.add(newNode)
+                    lastIndex = len(self.nodeList) - 1
+                    tempPath = self.getFinalCourse(lastIndex)
+                    tempPathLen = self.getPathLen(tempPath)
+                    if tempPathLen < pathLen:
+                        path = tempPath
+                        cBest = tempPathLen
+
+            if animation:
+                self.drawGraph(xCenter=xCenter,
+                               cBest=cBest, cMin=cMin,
+                               etheta=etheta, rnd=rnd)
+
+        return path
+
+    def chooseParent(self, newNode, nearInds):
+        if len(nearInds) == 0:
+            return newNode
+
+        dList = []
+        for i in nearInds:
+            dx = newNode.x - self.nodeList[i].x
+            dy = newNode.y - self.nodeList[i].y
+            d = math.sqrt(dx ** 2 + dy ** 2)
+            theta = math.atan2(dy, dx)
+            if self.check_collision_extend(self.nodeList[i], theta, d):
+                dList.append(self.nodeList[i].cost + d)
+            else:
+                dList.append(float('inf'))
+
+        minCost = min(dList)
+        minInd = nearInds[dList.index(minCost)]
+
+        if minCost == float('inf'):
+            print("mincost is inf")
+            return newNode
+
+        newNode.cost = minCost
+        newNode.parent = minInd
+
+        return newNode
+
+    def findNearNodes(self, newNode):
+        nnode = len(self.nodeList)
+        r = 50.0 * math.sqrt((math.log(nnode) / nnode))
+        dlist = [(node.x - newNode.x) ** 2 +
+                 (node.y - newNode.y) ** 2 for node in self.nodeList]
+        nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
+        return nearinds
+
+    def informed_sample(self, cMax, cMin, xCenter, C):
+        if cMax < float('inf'):
+            r = [cMax / 2.0,
+                 math.sqrt(cMax**2 - cMin**2) / 2.0,
+                 math.sqrt(cMax**2 - cMin**2) / 2.0]
+            L = np.diag(r)
+            xBall = self.sampleUnitBall()
+            rnd = np.dot(np.dot(C, L), xBall) + xCenter
+            rnd = [rnd[(0, 0)], rnd[(1, 0)]]
+        else:
+            rnd = self.sampleFreeSpace()
+
+        return rnd
+
+    def sampleUnitBall(self):
+        a = random.random()
+        b = random.random()
+
+        if b < a:
+            a, b = b, a
+
+        sample = (b * math.cos(2 * math.pi * a / b),
+                  b * math.sin(2 * math.pi * a / b))
+        return np.array([[sample[0]], [sample[1]], [0]])
+
+    def sampleFreeSpace(self):
+        if random.randint(0, 100) > self.goalSampleRate:
+            rnd = [random.uniform(self.minrand, self.maxrand),
+                   random.uniform(self.minrand, self.maxrand)]
+        else:
+            rnd = [self.goal.x, self.goal.y]
+
+        return rnd
+
+    def getPathLen(self, path):
+        pathLen = 0
+        for i in range(1, len(path)):
+            node1_x = path[i][0]
+            node1_y = path[i][1]
+            node2_x = path[i - 1][0]
+            node2_y = path[i - 1][1]
+            pathLen += math.sqrt((node1_x - node2_x) **
+                                 2 + (node1_y - node2_y)**2)
+
+        return pathLen
+
+    def lineCost(self, node1, node2):
+        return math.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2)
+
+    def getNearestListIndex(self, nodes, rnd):
+        dList = [(node.x - rnd[0])**2 +
+                 (node.y - rnd[1])**2 for node in nodes]
+        minIndex = dList.index(min(dList))
+        return minIndex
+
+    def __CollisionCheck(self, 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 getNewNode(self, theta, nind, nearestNode):
+        newNode = copy.deepcopy(nearestNode)
+
+        newNode.x += self.expandDis * math.cos(theta)
+        newNode.y += self.expandDis * math.sin(theta)
+
+        newNode.cost += self.expandDis
+        newNode.parent = nind
+        return newNode
+
+    def isNearGoal(self, node):
+        d = self.lineCost(node, self.goal)
+        if d < self.expandDis:
+            return True
+        return False
+
+    def rewire(self, newNode, nearInds):
+        nnode = len(self.nodeList)
+        for i in nearInds:
+            nearNode = self.nodeList[i]
+
+            d = math.sqrt((nearNode.x - newNode.x)**2 +
+                          (nearNode.y - newNode.y)**2)
+
+            scost = newNode.cost + d
+
+            if nearNode.cost > scost:
+                theta = math.atan2(newNode.y - nearNode.y,
+                                   newNode.x - nearNode.x)
+                if self.check_collision_extend(nearNode, theta, d):
+                    nearNode.parent = nnode - 1
+                    nearNode.cost = scost
+
+    def check_collision_extend(self, nearNode, theta, d):
+        tmpNode = copy.deepcopy(nearNode)
+
+        for i in range(int(d / self.expandDis)):
+            tmpNode.x += self.expandDis * math.cos(theta)
+            tmpNode.y += self.expandDis * math.sin(theta)
+            if not self.__CollisionCheck(tmpNode, self.obstacleList):
+                return False
+
+        return True
+
+    def getFinalCourse(self, lastIndex):
+        path = [[self.goal.x, self.goal.y]]
+        while self.nodeList[lastIndex].parent is not None:
+            node = self.nodeList[lastIndex]
+            path.append([node.x, node.y])
+            lastIndex = node.parent
+        path.append([self.start.x, self.start.y])
+        return path
+
+    def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None, rnd=None):
+
+        plt.clf()
+        if rnd is not None:
+            plt.plot(rnd[0], rnd[1], "^k")
+            if cBest != float('inf'):
+                self.plot_ellipse(xCenter, cBest, cMin, etheta)
+
+        for node in self.nodeList:
+            if node.parent is not None:
+                if node.x or node.y is not None:
+                    plt.plot([node.x, self.nodeList[node.parent].x], [
+                        node.y, self.nodeList[node.parent].y], "-g")
+
+        for (ox, oy, size) in self.obstacleList:
+            plt.plot(ox, oy, "ok", ms=30 * size)
+
+        plt.plot(self.start.x, self.start.y, "xr")
+        plt.plot(self.goal.x, self.goal.y, "xr")
+        plt.axis([-2, 15, -2, 15])
+        plt.grid(True)
+        plt.pause(0.01)
+
+    def plot_ellipse(self, xCenter, cBest, cMin, etheta):
+
+        a = math.sqrt(cBest**2 - cMin**2) / 2.0
+        b = cBest / 2.0
+        angle = math.pi / 2.0 - etheta
+        cx = xCenter[0]
+        cy = xCenter[1]
+
+        t = np.arange(0, 2 * math.pi + 0.1, 0.1)
+        x = [a * math.cos(it) for it in t]
+        y = [b * math.sin(it) for it in t]
+        R = np.matrix([[math.cos(angle), math.sin(angle)],
+                       [-math.sin(angle), math.cos(angle)]])
+        fx = R * np.matrix([x, y])
+        px = np.array(fx[0, :] + cx).flatten()
+        py = np.array(fx[1, :] + cy).flatten()
+        plt.plot(cx, cy, "xc")
+        plt.plot(px, py, "--c")
+
+
+class Node():
+
+    def __init__(self, x, y):
+        self.x = x
+        self.y = y
+        self.cost = 0.0
+        self.parent = None
+
+
+def main():
+    print("Start informed rrt star planning")
+
+    # create obstacles
+    obstacleList = [
+        (5, 5, 0.5),
+        (9, 6, 1),
+        (7, 5, 1),
+        (1, 5, 1),
+        (3, 6, 1),
+        (7, 9, 1)
+    ]
+
+    # Set params
+    rrt = InformedRRTStar(start=[0, 0], goal=[5, 10],
+                          randArea=[-2, 15], obstacleList=obstacleList)
+    path = rrt.InformedRRTStarSearch(animation=show_animation)
+    print("Done!!")
+
+    # Plot path
+    if show_animation:
+        rrt.drawGraph()
+        plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
+        plt.grid(True)
+        plt.pause(0.01)
+        plt.show()
+
+
+if __name__ == '__main__':
+    main()

PathPlanning/LQRPlanner/LQRplanner.py

@@ -0,0 +1,160 @@
+"""
+
+LQR local path planning
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+"""
+
+import matplotlib.pyplot as plt
+import numpy as np
+import scipy.linalg as la
+import math
+import random
+
+show_animation = True
+
+MAX_TIME = 100.0  # Maximum simulation time
+DT = 0.1  # Time tick
+
+
+def LQRplanning(sx, sy, gx, gy):
+
+    rx, ry = [sx], [sy]
+
+    x = np.matrix([sx - gx, sy - gy]).T  # State vector
+
+    # Linear system model
+    A, B = get_system_model()
+
+    found_path = False
+
+    time = 0.0
+    while time <= MAX_TIME:
+        time += DT
+
+        u = LQR_control(A, B, x)
+
+        x = A * x + B * u
+
+        rx.append(x[0, 0] + gx)
+        ry.append(x[1, 0] + gy)
+
+        d = math.sqrt((gx - rx[-1])**2 + (gy - ry[-1])**2)
+        if d <= 0.1:
+            #  print("Goal!!")
+            found_path = True
+            break
+
+        # animation
+        if show_animation:
+            plt.plot(sx, sy, "or")
+            plt.plot(gx, gy, "ob")
+            plt.plot(rx, ry, "-r")
+            plt.axis("equal")
+            plt.pause(1.0)
+
+    if not found_path:
+        print("Cannot found path")
+        return [], []
+
+    return rx, ry
+
+
+def solve_DARE(A, B, Q, R):
+    """
+    solve a discrete time_Algebraic Riccati equation (DARE)
+    """
+    X = Q
+    maxiter = 150
+    eps = 0.01
+
+    for i in range(maxiter):
+        Xn = A.T * X * A - A.T * X * B * \
+            la.inv(R + B.T * X * B) * B.T * X * A + Q
+        if (abs(Xn - X)).max() < eps:
+            X = Xn
+            break
+        X = Xn
+
+    return Xn
+
+
+def dlqr(A, B, Q, R):
+    """Solve the discrete time lqr controller.
+    x[k+1] = A x[k] + B u[k]
+    cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
+    # ref Bertsekas, p.151
+    """
+
+    # first, try to solve the ricatti equation
+    X = solve_DARE(A, B, Q, R)
+
+    # compute the LQR gain
+    K = np.matrix(la.inv(B.T * X * B + R) * (B.T * X * A))
+
+    eigVals, eigVecs = la.eig(A - B * K)
+
+    return K, X, eigVals
+
+
+def get_system_model():
+
+    A = np.matrix([[DT, 1.0],
+                   [0.0, DT]])
+    B = np.matrix([0.0, 1.0]).T
+
+    return A, B
+
+
+def LQR_control(A, B, x):
+
+    Kopt, X, ev = dlqr(A, B, np.eye(2), np.eye(1))
+
+    u = -Kopt * x
+
+    return u
+
+
+def main():
+    print(__file__ + " start!!")
+
+    ntest = 10  # number of goal
+    area = 100.0  # sampling area
+
+    for i in range(ntest):
+        sx = 6.0
+        sy = 6.0
+        gx = random.uniform(-area, area)
+        gy = random.uniform(-area, area)
+
+        rx, ry = LQRplanning(sx, sy, gx, gy)
+
+        if show_animation:
+            plt.plot(sx, sy, "or")
+            plt.plot(gx, gy, "ob")
+            plt.plot(rx, ry, "-r")
+            plt.axis("equal")
+            plt.pause(1.0)
+
+
+def main1():
+    print(__file__ + " start!!")
+
+    sx = 6.0
+    sy = 6.0
+    gx = 10.0
+    gy = 10.0
+
+    rx, ry = LQRplanning(sx, sy, gx, gy)
+
+    if show_animation:
+        plt.plot(sx, sy, "or")
+        plt.plot(gx, gy, "ob")
+        plt.plot(rx, ry, "-r")
+        plt.axis("equal")
+        plt.show()
+
+
+if __name__ == '__main__':
+    main()

PathPlanning/LQRRRTStar/lqr_rrt_star.py

@@ -0,0 +1,314 @@
+"""
+
+Path planning code with LQR RRT*
+
+author: AtsushiSakai(@Atsushi_twi)
+
+"""
+
+import sys
+sys.path.append("../LQRPlanner/")
+
+import random
+import math
+import copy
+import numpy as np
+import matplotlib.pyplot as plt
+import LQRplanner
+
+show_animation = True
+
+LQRplanner.show_animation = False
+
+STEP_SIZE = 0.05  # step size of local path
+XYTH = 0.5  # [m] acceptance xy distance in final paths
+
+
+class RRT():
+    """
+    Class for RRT Planning
+    """
+
+    def __init__(self, start, goal, obstacleList, randArea,
+                 goalSampleRate=10, maxIter=200):
+        """
+        Setting Parameter
+
+        start:Start Position [x,y]
+        goal:Goal Position [x,y]
+        obstacleList:obstacle Positions [[x,y,size],...]
+        randArea:Ramdom Samping Area [min,max]
+
+        """
+        self.start = Node(start[0], start[1])
+        self.end = Node(goal[0], goal[1])
+        self.minrand = randArea[0]
+        self.maxrand = randArea[1]
+        self.goalSampleRate = goalSampleRate
+        self.maxIter = maxIter
+        self.obstacleList = obstacleList
+
+    def planning(self, animation=True):
+        """
+        Pathplanning
+
+        animation: flag for animation on or off
+        """
+
+        self.nodeList = [self.start]
+        for i in range(self.maxIter):
+            rnd = self.get_random_point()
+            nind = self.get_nearest_index(self.nodeList, rnd)
+
+            newNode = self.steer(rnd, nind)
+            if newNode is None:
+                continue
+
+            if self.check_collision(newNode, self.obstacleList):
+                nearinds = self.find_near_nodes(newNode)
+                newNode = self.choose_parent(newNode, nearinds)
+                if newNode is None:
+                    continue
+                self.nodeList.append(newNode)
+                self.rewire(newNode, nearinds)
+
+            if animation and i % 5 == 0:
+                self.draw_graph(rnd=rnd)
+
+        # generate coruse
+        lastIndex = self.get_best_last_index()
+        if lastIndex is None:
+            return None
+        path = self.gen_final_course(lastIndex)
+        return path
+
+    def choose_parent(self, newNode, nearinds):
+        if len(nearinds) == 0:
+            return newNode
+
+        dlist = []
+        for i in nearinds:
+            tNode = self.steer(newNode, i)
+            if tNode is None:
+                continue
+
+            if self.check_collision(tNode, self.obstacleList):
+                dlist.append(tNode.cost)
+            else:
+                dlist.append(float("inf"))
+
+        mincost = min(dlist)
+        minind = nearinds[dlist.index(mincost)]
+
+        if mincost == float("inf"):
+            print("mincost is inf")
+            return newNode
+
+        newNode = self.steer(newNode, minind)
+
+        return newNode
+
+    def pi_2_pi(self, angle):
+        return (angle + math.pi) % (2*math.pi) - math.pi
+
+    def sample_path(self, wx, wy, step):
+
+        px, py, clen = [], [], []
+
+        for i in range(len(wx) - 1):
+
+            for t in np.arange(0.0, 1.0, step):
+                px.append(t * wx[i + 1] + (1.0 - t) * wx[i])
+                py.append(t * wy[i + 1] + (1.0 - t) * wy[i])
+
+        dx = np.diff(px)
+        dy = np.diff(py)
+
+        clen = [math.sqrt(idx**2 + idy**2) for (idx, idy) in zip(dx, dy)]
+
+        return px, py, clen
+
+    def steer(self, rnd, nind):
+
+        nearestNode = self.nodeList[nind]
+
+        wx, wy = LQRplanner.LQRplanning(
+            nearestNode.x, nearestNode.y, rnd.x, rnd.y)
+
+        px, py, clen = self.sample_path(wx, wy, STEP_SIZE)
+
+        if px is None:
+            return None
+
+        newNode = copy.deepcopy(nearestNode)
+        newNode.x = px[-1]
+        newNode.y = py[-1]
+
+        newNode.path_x = px
+        newNode.path_y = py
+        newNode.cost += sum([abs(c) for c in clen])
+        newNode.parent = nind
+
+        return newNode
+
+    def get_random_point(self):
+
+        if random.randint(0, 100) > self.goalSampleRate:
+            rnd = [random.uniform(self.minrand, self.maxrand),
+                   random.uniform(self.minrand, self.maxrand),
+                   random.uniform(-math.pi, math.pi)
+                   ]
+        else:  # goal point sampling
+            rnd = [self.end.x, self.end.y]
+
+        node = Node(rnd[0], rnd[1])
+
+        return node
+
+    def get_best_last_index(self):
+        #  print("get_best_last_index")
+
+        goalinds = []
+        for (i, node) in enumerate(self.nodeList):
+            if self.calc_dist_to_goal(node.x, node.y) <= XYTH:
+                goalinds.append(i)
+
+        if len(goalinds) == 0:
+            return None
+
+        mincost = min([self.nodeList[i].cost for i in goalinds])
+        for i in goalinds:
+            if self.nodeList[i].cost == mincost:
+                return i
+
+        return None
+
+    def gen_final_course(self, goalind):
+        path = [[self.end.x, self.end.y]]
+        while self.nodeList[goalind].parent is not None:
+            node = self.nodeList[goalind]
+            for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
+                path.append([ix, iy])
+            goalind = node.parent
+        path.append([self.start.x, self.start.y])
+        return path
+
+    def calc_dist_to_goal(self, x, y):
+        return np.linalg.norm([x - self.end.x, y - self.end.y])
+
+    def find_near_nodes(self, newNode):
+        nnode = len(self.nodeList)
+        r = 50.0 * math.sqrt((math.log(nnode) / nnode))
+        dlist = [(node.x - newNode.x) ** 2 +
+                 (node.y - newNode.y) ** 2
+                 for node in self.nodeList]
+        nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
+        return nearinds
+
+    def rewire(self, newNode, nearinds):
+
+        nnode = len(self.nodeList)
+
+        for i in nearinds:
+            nearNode = self.nodeList[i]
+            tNode = self.steer(nearNode, nnode - 1)
+            if tNode is None:
+                continue
+
+            obstacleOK = self.check_collision(tNode, self.obstacleList)
+            imporveCost = nearNode.cost > tNode.cost
+
+            if obstacleOK and imporveCost:
+                #  print("rewire")
+                self.nodeList[i] = tNode
+
+    def draw_graph(self, rnd=None):
+        plt.clf()
+        if rnd is not None:
+            plt.plot(rnd.x, rnd.y, "^k")
+
+        for node in self.nodeList:
+            if node.parent is not None:
+                plt.plot(node.path_x, node.path_y, "-g")
+
+        for (ox, oy, size) in self.obstacleList:
+            plt.plot(ox, oy, "ok", ms=30 * size)
+
+        plt.plot(self.start.x, self.start.y, "or")
+        plt.plot(self.end.x, self.end.y, "or")
+
+        plt.axis([-2, 15, -2, 15])
+        plt.grid(True)
+        plt.pause(0.01)
+
+    def get_nearest_index(self, nodeList, rnd):
+        dlist = [(node.x - rnd.x) ** 2 +
+                 (node.y - rnd.y) ** 2
+                 for node in nodeList]
+        minind = dlist.index(min(dlist))
+
+        return minind
+
+    def check_collision(self, node, obstacleList):
+
+        px = np.array(node.path_x)
+        py = np.array(node.path_y)
+
+        for (ox, oy, size) in obstacleList:
+            dx = ox - px
+            dy = oy - py
+            d = dx ** 2 + dy ** 2
+            dmin = min(d)
+            if dmin <= size ** 2:
+                return False  # collision
+
+        return True  # safe
+
+
+class Node():
+    """
+    RRT Node
+    """
+
+    def __init__(self, x, y):
+        self.x = x
+        self.y = y
+        self.path_x = []
+        self.path_y = []
+        self.cost = 0.0
+        self.parent = None
+
+
+def main():
+    print("Start rrt start planning")
+
+    # ====Search Path with RRT====
+    obstacleList = [
+        (5, 5, 1),
+        (4, 6, 1),
+        (4, 7.5, 1),
+        (4, 9, 1),
+        (6, 5, 1),
+        (7, 5, 1)
+    ]  # [x,y,size]
+
+    # Set Initial parameters
+    start = [0.0, 0.0]
+    goal = [6.0, 7.0]
+
+    rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
+    path = rrt.planning(animation=show_animation)
+
+    # Draw final path
+    if show_animation:
+        rrt.draw_graph()
+        plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
+        plt.grid(True)
+        plt.pause(0.001)
+        plt.show()
+
+    print("Done")
+
+
+if __name__ == '__main__':
+    main()

PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py

@@ -18,13 +18,7 @@ class State:
 
 
 def pi_2_pi(angle):
-    while(angle > math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while(angle < -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
+    return (angle + math.pi) % (2*math.pi) - math.pi
 
 
 def update(state, v, delta, dt, L):

PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py

@@ -15,8 +15,8 @@ import matplotlib.pyplot as plt
 import math
 
 # parameter
-MAX_T = 100.0
-MIN_T = 5.0
+MAX_T = 100.0  # maximum time to the goal [s]
+MIN_T = 5.0  # minimum time to the goal[s]
 
 show_animation = True
 
@@ -66,8 +66,13 @@ class quinic_polynomial:
 
         return xt
 
+    def calc_third_derivative(self, t):
+        xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t**2
 
-def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, dt):
+        return xt
+
+
+def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):
     """
     quinic polynomial planner
 
@@ -81,6 +86,7 @@ def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_a
         gyaw: goal yaw angle [rad]
         ga: goal accel [m/ss]
         max_accel: maximum accel [m/ss]
+        max_jerk: maximum jerk [m/sss]
         dt: time tick [s]
 
     return
@@ -107,7 +113,7 @@ def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_a
         xqp = quinic_polynomial(sx, vxs, axs, gx, vxg, axg, T)
         yqp = quinic_polynomial(sy, vys, ays, gy, vyg, ayg, T)
 
-        time, rx, ry, ryaw, rv, ra = [], [], [], [], [], []
+        time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []
 
         for t in np.arange(0.0, T + dt, dt):
             time.append(t)
@@ -126,10 +132,16 @@ def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_a
             a = np.hypot(ax, ay)
             if len(rv) >= 2 and rv[-1] - rv[-2] < 0.0:
                 a *= -1
-                pass
             ra.append(a)
 
-        if max([abs(i) for i in ra]) <= max_accel:
+            jx = xqp.calc_third_derivative(t)
+            jy = yqp.calc_third_derivative(t)
+            j = np.hypot(jx, jy)
+            if len(ra) >= 2 and ra[-1] - ra[-2] < 0.0:
+                j *= -1
+            rj.append(j)
+
+        if max([abs(i) for i in ra]) <= max_accel and max([abs(i) for i in rj]) <= max_jerk:
             print("find path!!")
             break
 
@@ -143,14 +155,16 @@ def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_a
             plot_arrow(rx[i], ry[i], ryaw[i])
             plt.title("Time[s]:" + str(time[i])[0:4] +
                       " v[m/s]:" + str(rv[i])[0:4] +
-                      " a[m/ss]:" + str(ra[i])[0:4])
+                      " a[m/ss]:" + str(ra[i])[0:4] +
+                      " jerk[m/sss]:" + str(rj[i])[0:4],
+                      )
             plt.pause(0.001)
 
-    return time, rx, ry, ryaw, rv, ra
+    return time, rx, ry, ryaw, rv, ra, rj
 
 
 def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
-    u"""
+    """
     Plot arrow
     """
 
@@ -177,10 +191,11 @@ def main():
     gv = 1.0  # goal speed [m/s]
     ga = 0.1  # goal accel [m/ss]
     max_accel = 1.0  # max accel [m/ss]
+    max_jerk = 0.5  # max jerk [m/sss]
     dt = 0.1  # time tick [s]
 
-    time, x, y, yaw, v, a = quinic_polynomials_planner(
-        sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, dt)
+    time, x, y, yaw, v, a, j = quinic_polynomials_planner(
+        sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt)
 
     if show_animation:
         plt.plot(x, y, "-r")
@@ -203,6 +218,12 @@ def main():
         plt.ylabel("accel[m/ss]")
         plt.grid(True)
 
+        plt.subplots()
+        plt.plot(time, j, "-r")
+        plt.xlabel("Time[s]")
+        plt.ylabel("jerk[m/sss]")
+        plt.grid(True)
+
         plt.show()
 
 

PathPlanning/RRT/simple_rrt.py

@@ -39,7 +39,7 @@ class RRT():
         self.obstacleList = obstacleList
 
     def Planning(self, animation=True):
-        u"""
+        """
         Pathplanning
 
         animation: flag for animation on or off
@@ -71,6 +71,7 @@ class RRT():
                 continue
 
             self.nodeList.append(newNode)
+            print("nNodelist:", len(self.nodeList))
 
             # check goal
             dx = newNode.x - self.end.x
@@ -94,7 +95,7 @@ class RRT():
         return path
 
     def DrawGraph(self, rnd=None):
-        u"""
+        """
         Draw Graph
         """
         plt.clf()
@@ -133,7 +134,7 @@ class RRT():
 
 
 class Node():
-    u"""
+    """
     RRT Node
     """
 
@@ -144,7 +145,7 @@ class Node():
 
 
 def main():
-    print("start RRT path planning")
+    print("start simple RRT path planning")
 
     # ====Search Path with RRT====
     obstacleList = [

PathPlanning/RRTDubins/dubins_path_planning.py

@@ -17,13 +17,7 @@ def mod2pi(theta):
 
 
 def pi_2_pi(angle):
-    while(angle >= math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while(angle <= -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
+    return (angle + math.pi) % (2*math.pi) - math.pi
 
 
 def LSL(alpha, beta, d):

PathPlanning/RRTDubins/rrt_dubins.py

@@ -96,14 +96,10 @@ class RRT():
 
         return newNode
 
+
     def pi_2_pi(self, angle):
-        while(angle >= math.pi):
-            angle = angle - 2.0 * math.pi
+        return (angle + math.pi) % (2*math.pi) - math.pi
 
-        while(angle <= -math.pi):
-            angle = angle + 2.0 * math.pi
-
-        return angle
 
     def steer(self, rnd, nind):
         #  print(rnd)

PathPlanning/RRTStarDubins/dubins_path_planning.py

@@ -13,13 +13,7 @@ def mod2pi(theta):
 
 
 def pi_2_pi(angle):
-    while(angle >= math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while(angle <= -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
+    return (angle + math.pi) % (2*math.pi) - math.pi
 
 
 def LSL(alpha, beta, d):

PathPlanning/RRTStarDubins/rrt_star_dubins.py

@@ -97,13 +97,7 @@ class RRT():
         return newNode
 
     def pi_2_pi(self, angle):
-        while(angle >= math.pi):
-            angle = angle - 2.0 * math.pi
-
-        while(angle <= -math.pi):
-            angle = angle + 2.0 * math.pi
-
-        return angle
+        return (angle + math.pi) % (2*math.pi) - math.pi
 
     def steer(self, rnd, nind):
         #  print(rnd)

PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py

@@ -106,13 +106,7 @@ class RRT():
         return newNode
 
     def pi_2_pi(self, angle):
-        while(angle > math.pi):
-            angle = angle - 2.0 * math.pi
-
-        while(angle < -math.pi):
-            angle = angle + 2.0 * math.pi
-
-        return angle
+        return (angle + math.pi) % (2*math.pi) - math.pi
 
     def steer(self, rnd, nind):
 

PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py

@@ -331,13 +331,7 @@ def generate_local_course(L, lengths, mode, maxc, step_size):
 
 
 def pi_2_pi(angle):
-    while(angle > math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while(angle < -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
+    return (angle + math.pi) % (2*math.pi) - math.pi
 
 
 def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size):

PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py

@@ -51,13 +51,7 @@ def update(state, a, delta):
 
 
 def pi_2_pi(angle):
-    while (angle > math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while (angle < -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
+    return (angle + math.pi) % (2*math.pi) - math.pi
 
 
 def solve_DARE(A, B, Q, R):
@@ -148,12 +142,14 @@ def calc_nearest_index(state, cx, cy, cyaw):
     dx = [state.x - icx for icx in cx]
     dy = [state.y - icy for icy in cy]
 
-    d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
+    d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
 
     mind = min(d)
 
     ind = d.index(mind)
 
+    mind = math.sqrt(mind)
+
     dxl = cx[ind] - state.x
     dyl = cy[ind] - state.y
 

PathTracking/lqr_steer_control/lqr_steer_control.py

@@ -61,13 +61,7 @@ def PIDControl(target, current):
 
 
 def pi_2_pi(angle):
-    while (angle > math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while (angle < -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
+    return (angle + math.pi) % (2*math.pi) - math.pi
 
 
 def solve_DARE(A, B, Q, R):
@@ -146,12 +140,14 @@ def calc_nearest_index(state, cx, cy, cyaw):
     dx = [state.x - icx for icx in cx]
     dy = [state.y - icy for icy in cy]
 
-    d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
+    d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
 
     mind = min(d)
 
     ind = d.index(mind)
 
+    mind = math.sqrt(mind)
+
     dxl = cx[ind] - state.x
     dyl = cy[ind] - state.y
 

PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py

@@ -88,16 +88,16 @@ def get_linear_model_matrix(v, phi, delta):
     A[0, 3] = - DT * v * math.sin(phi)
     A[1, 2] = DT * math.sin(phi)
     A[1, 3] = DT * v * math.cos(phi)
-    A[3, 2] = DT * math.tan(delta)
+    A[3, 2] = DT * math.tan(delta) / WB
 
     B = np.matrix(np.zeros((NX, NU)))
     B[2, 0] = DT
     B[3, 1] = DT * v / (WB * math.cos(delta) ** 2)
 
-    C = np.matrix(np.zeros((NX, 1)))
-    C[0, 0] = DT * v * math.sin(phi) * phi
-    C[1, 0] = - DT * v * math.cos(phi) * phi
-    C[3, 0] = v * delta / (WB * math.cos(delta) ** 2)
+    C = np.zeros(NX)
+    C[0] = DT * v * math.sin(phi) * phi
+    C[1] = - DT * v * math.cos(phi) * phi
+    C[3] = v * delta / (WB * math.cos(delta) ** 2)
 
     return A, B, C
 
@@ -188,20 +188,22 @@ def calc_nearest_index(state, cx, cy, cyaw, pind):
     dx = [state.x - icx for icx in cx[pind:(pind + N_IND_SEARCH)]]
     dy = [state.y - icy for icy in cy[pind:(pind + N_IND_SEARCH)]]
 
-    d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
+    d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
 
-    min_d = min(d)
+    mind = min(d)
 
-    ind = d.index(min_d) + pind
+    ind = d.index(mind) + pind
+
+    mind = math.sqrt(mind)
 
     dxl = cx[ind] - state.x
     dyl = cy[ind] - state.y
 
     angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl))
     if angle < 0:
-        min_d *= -1
+        mind *= -1
 
-    return ind, min_d
+    return ind, mind
 
 
 def predict_motion(x0, oa, od, xref):
@@ -252,8 +254,8 @@ def linear_mpc_control(xref, xbar, x0, dref):
     dref: reference steer angle
     """
 
-    x = cvxpy.Variable(NX, T + 1)
-    u = cvxpy.Variable(NU, T)
+    x = cvxpy.Variable((NX, T + 1))
+    u = cvxpy.Variable((NU, T))
 
     cost = 0.0
     constraints = []
@@ -271,18 +273,18 @@ def linear_mpc_control(xref, xbar, x0, dref):
         if t < (T - 1):
             cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd)
             constraints += [cvxpy.abs(u[1, t + 1] - u[1, t])
-                            < MAX_DSTEER * DT]
+                            <= MAX_DSTEER * DT]
 
     cost += cvxpy.quad_form(xref[:, T] - x[:, T], Qf)
 
     constraints += [x[:, 0] == x0]
     constraints += [x[2, :] <= MAX_SPEED]
     constraints += [x[2, :] >= MIN_SPEED]
-    constraints += [cvxpy.abs(u[0, :]) < MAX_ACCEL]
-    constraints += [cvxpy.abs(u[1, :]) < MAX_STEER]
+    constraints += [cvxpy.abs(u[0, :]) <= MAX_ACCEL]
+    constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER]
 
     prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints)
-    prob.solve(verbose=False)
+    prob.solve(solver=cvxpy.ECOS, verbose=False)
 
     if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE:
         ox = get_nparray_from_matrix(x.value[0, :])
@@ -363,7 +365,7 @@ def check_goal(state, goal, tind, nind):
     return False
 
 
-def do_simulation(cx, cy, cyaw, ck, sp, dl):
+def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state):
     """
     Simulation
 
@@ -377,7 +379,14 @@ def do_simulation(cx, cy, cyaw, ck, sp, dl):
     """
 
     goal = [cx[-1], cy[-1]]
-    state = State(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0)
+
+    state = initial_state
+
+    # initial yaw compensation
+    if state.yaw - cyaw[0] >= math.pi:
+        state.yaw -= math.pi * 2.0
+    elif state.yaw - cyaw[0] <= -math.pi:
+        state.yaw += math.pi * 2.0
 
     time = 0.0
     x = [state.x]
@@ -471,9 +480,11 @@ def smooth_yaw(yaw):
 
     for i in range(len(yaw) - 1):
         dyaw = yaw[i + 1] - yaw[i]
+
         while dyaw >= math.pi / 2.0:
             yaw[i + 1] -= math.pi * 2.0
             dyaw = yaw[i + 1] - yaw[i]
+
         while dyaw <= -math.pi / 2.0:
             yaw[i + 1] += math.pi * 2.0
             dyaw = yaw[i + 1] - yaw[i]
@@ -481,6 +492,35 @@ def smooth_yaw(yaw):
     return yaw
 
 
+def get_straight_course(dl):
+    ax = [0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0]
+    ay = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
+        ax, ay, ds=dl)
+
+    return cx, cy, cyaw, ck
+
+
+def get_straight_course2(dl):
+    ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0]
+    ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0]
+    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
+        ax, ay, ds=dl)
+
+    return cx, cy, cyaw, ck
+
+
+def get_straight_course3(dl):
+    ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0]
+    ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0]
+    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
+        ax, ay, ds=dl)
+
+    cyaw = [i - math.pi for i in cyaw]
+
+    return cx, cy, cyaw, ck
+
+
 def get_forward_course(dl):
     ax = [0.0, 60.0, 125.0, 50.0, 75.0, 30.0, -10.0]
     ay = [0.0, 0.0, 50.0, 65.0, 30.0, 50.0, -20.0]
@@ -512,12 +552,51 @@ def main():
     print(__file__ + " start!!")
 
     dl = 1.0  # course tick
-    #  cx, cy, cyaw, ck = get_forward_course(dl)
-    cx, cy, cyaw, ck = get_switch_back_course(dl)
+    # cx, cy, cyaw, ck = get_straight_course(dl)
+    # cx, cy, cyaw, ck = get_straight_course2(dl)
+    cx, cy, cyaw, ck = get_straight_course3(dl)
+    # cx, cy, cyaw, ck = get_forward_course(dl)
+    # CX, cy, cyaw, ck = get_switch_back_course(dl)
 
     sp = calc_speed_profile(cx, cy, cyaw, TARGET_SPEED)
 
-    t, x, y, yaw, v, d, a = do_simulation(cx, cy, cyaw, ck, sp, dl)
+    initial_state = State(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0)
+
+    t, x, y, yaw, v, d, a = do_simulation(
+        cx, cy, cyaw, ck, sp, dl, initial_state)
+
+    if show_animation:
+        plt.close("all")
+        plt.subplots()
+        plt.plot(cx, cy, "-r", label="spline")
+        plt.plot(x, y, "-g", label="tracking")
+        plt.grid(True)
+        plt.axis("equal")
+        plt.xlabel("x[m]")
+        plt.ylabel("y[m]")
+        plt.legend()
+
+        plt.subplots()
+        plt.plot(t, v, "-r", label="speed")
+        plt.grid(True)
+        plt.xlabel("Time [s]")
+        plt.ylabel("Speed [kmh]")
+
+        plt.show()
+
+
+def main2():
+    print(__file__ + " start!!")
+
+    dl = 1.0  # course tick
+    cx, cy, cyaw, ck = get_straight_course3(dl)
+
+    sp = calc_speed_profile(cx, cy, cyaw, TARGET_SPEED)
+
+    initial_state = State(x=cx[0], y=cy[0], yaw=0.0, v=0.0)
+
+    t, x, y, yaw, v, d, a = do_simulation(
+        cx, cy, cyaw, ck, sp, dl, initial_state)
 
     if show_animation:
         plt.close("all")
@@ -540,4 +619,5 @@ def main():
 
 
 if __name__ == '__main__':
-    main()
+    # main()
+    main2()

PathTracking/model_predictive_speed_and_steer_control/notebook.ipynb

@@ -0,0 +1,301 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Model predictive speed and steering control\n",
+    "\n",
+    "code:\n",
+    "\n",
+    "https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py\n",
+    "\n",
+    "This is a path tracking simulation using model predictive control (MPC).\n",
+    "\n",
+    "The MPC controller controls vehicle speed and steering base on linealized model.\n",
+    "\n",
+    "This code uses cvxpy as an optimization modeling tool.\n",
+    "\n",
+    "- [Welcome to CVXPY 1\\.0 — CVXPY 1\\.0\\.6 documentation](http://www.cvxpy.org/)"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# MPC modeling\n",
+    "\n",
+    "State vector is:\n",
+    "$$ z = [x, y, v,\\phi]$$ x: x-position, y:y-position, v:velocity, φ: yaw angle\n",
+    "\n",
+    "Input vector is:\n",
+    "$$ u = [a, \\delta]$$ a: accellation, δ: steering angle\n",
+    "\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "The MPC cotroller minimize this cost function for path tracking:\n",
+    "\n",
+    "$$min\\ Q_f(z_{T,ref}-z_{T})^2+Q\\Sigma({z_{t,ref}-z_{t}})^2+R\\Sigma{u_t}^2+R_d\\Sigma({u_{t+1}-u_{t}})^2$$\n",
+    "\n",
+    "z_ref come from target path and speed."
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "subject to:\n",
+    "- Linearlied vehicle model\n",
+    "$$z_{t+1}=Az_t+Bu+C$$\n",
+    "- Maximum steering speed\n",
+    "$$|u_{t+1}-u_{t}|<du_{max}$$\n",
+    "- Maximum steering angle\n",
+    "$$|u_{t}|<u_{max}$$\n",
+    "- Initial state\n",
+    "$$z_0 = z_{0,ob}$$\n",
+    "- Maximum and minimum speed\n",
+    "$$v_{min} < v_t < v_{max}$$\n",
+    "- Maximum and minimum input\n",
+    "$$u_{min} < u_t < u_{max}$$\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "This is implemented at \n",
+    "\n",
+    "https://github.com/AtsushiSakai/PythonRobotics/blob/f51a73f47cb922a12659f8ce2d544c347a2a8156/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py#L247-L301"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Vehicle model linearization\n",
+    "\n",
+    "\n",
+    "\n",
+    "Vehicle model is \n",
+    "$$ \\dot{x} = vcos(\\phi)$$\n",
+    "$$ \\dot{y} = vsin((\\phi)$$\n",
+    "$$ \\dot{v} = a$$\n",
+    "$$ \\dot{\\phi} = \\frac{vtan(\\delta)}{L}$$\n",
+    "\n",
+    "\n",
+    "\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "ODE is\n",
+    "\n",
+    "$$ \\dot{z} =\\frac{\\partial }{\\partial z} z = f(z, u) = A'z+B'u$$\n",
+    "\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "where\n",
+    "\n",
+    "\\begin{equation*}\n",
+    "A' =\n",
+    "\\begin{bmatrix}\n",
+    "\\frac{\\partial }{\\partial x}vcos(\\phi) & \n",
+    "\\frac{\\partial }{\\partial y}vcos(\\phi) & \n",
+    "\\frac{\\partial }{\\partial v}vcos(\\phi) &\n",
+    "\\frac{\\partial }{\\partial \\phi}vcos(\\phi)\\\\\n",
+    "\\frac{\\partial }{\\partial x}vsin(\\phi) & \n",
+    "\\frac{\\partial }{\\partial y}vsin(\\phi) & \n",
+    "\\frac{\\partial }{\\partial v}vsin(\\phi) &\n",
+    "\\frac{\\partial }{\\partial \\phi}vsin(\\phi)\\\\\n",
+    "\\frac{\\partial }{\\partial x}a& \n",
+    "\\frac{\\partial }{\\partial y}a& \n",
+    "\\frac{\\partial }{\\partial v}a&\n",
+    "\\frac{\\partial }{\\partial \\phi}a\\\\\n",
+    "\\frac{\\partial }{\\partial x}\\frac{vtan(\\delta)}{L}& \n",
+    "\\frac{\\partial }{\\partial y}\\frac{vtan(\\delta)}{L}& \n",
+    "\\frac{\\partial }{\\partial v}\\frac{vtan(\\delta)}{L}&\n",
+    "\\frac{\\partial }{\\partial \\phi}\\frac{vtan(\\delta)}{L}\\\\\n",
+    "\\end{bmatrix}\n",
+    "\\\\\n",
+    "　=\n",
+    "\\begin{bmatrix}\n",
+    "0 & 0 & cos(\\bar{\\phi}) & -\\bar{v}sin(\\bar{\\phi})\\\\\n",
+    "0 & 0 & sin(\\bar{\\phi}) & \\bar{v}cos(\\bar{\\phi}) \\\\\n",
+    "0 & 0 & 0 & 0 \\\\\n",
+    "0 & 0 &\\frac{tan(\\bar{\\delta})}{L} & 0 \\\\\n",
+    "\\end{bmatrix}\n",
+    "\\end{equation*}\n",
+    "\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "\\begin{equation*}\n",
+    "B' =\n",
+    "\\begin{bmatrix}\n",
+    "\\frac{\\partial }{\\partial a}vcos(\\phi) &\n",
+    "\\frac{\\partial }{\\partial \\delta}vcos(\\phi)\\\\\n",
+    "\\frac{\\partial }{\\partial a}vsin(\\phi) &\n",
+    "\\frac{\\partial }{\\partial \\delta}vsin(\\phi)\\\\\n",
+    "\\frac{\\partial }{\\partial a}a &\n",
+    "\\frac{\\partial }{\\partial \\delta}a\\\\\n",
+    "\\frac{\\partial }{\\partial a}\\frac{vtan(\\delta)}{L} &\n",
+    "\\frac{\\partial }{\\partial \\delta}\\frac{vtan(\\delta)}{L}\\\\\n",
+    "\\end{bmatrix}\n",
+    "\\\\\n",
+    "　=\n",
+    "\\begin{bmatrix}\n",
+    "0 & 0 \\\\\n",
+    "0 & 0 \\\\\n",
+    "1 & 0 \\\\\n",
+    "0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})} \\\\\n",
+    "\\end{bmatrix}\n",
+    "\\end{equation*}\n",
+    "\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "You can get a discrete-time mode with Forward Euler Discretization with sampling time dt.\n",
+    "\n",
+    "$$z_{k+1}=z_k+f(z_k,u_k)dt$$\n",
+    "\n",
+    "Using first degree Tayer expantion around zbar and ubar\n",
+    "$$z_{k+1}=z_k+(f(\\bar{z},\\bar{u})+A'z_k+B'u_k)dt$$\n",
+    "\n",
+    "$$z_{k+1}=(I + dtA')z_k+(dtB')u_k + (f(\\bar{z},\\bar{u})-A'\\bar{z}+B'\\bar{u})dt$$\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "So, \n",
+    "\n",
+    "$$z_{k+1}=Az_k+Bu_k +C$$\n",
+    "\n",
+    "where,\n",
+    "\n",
+    "\\begin{equation*}\n",
+    "A = (I + dtA')\\\\\n",
+    "=\n",
+    "\\begin{bmatrix} \n",
+    "1 & 0 & cos(\\bar{\\phi})dt & -\\bar{v}sin(\\bar{\\phi})dt\\\\\n",
+    "0 & 1 & sin(\\bar{\\phi})dt & \\bar{v}cos(\\bar{\\phi})dt \\\\\n",
+    "0 & 0 & 1 & 0 \\\\\n",
+    "0 & 0 &\\frac{tan(\\bar{\\delta})}{L}dt & 1 \\\\\n",
+    "\\end{bmatrix}\n",
+    "\\end{equation*}"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "\\begin{equation*}\n",
+    "B = dtB'\\\\\n",
+    "=\n",
+    "\\begin{bmatrix} \n",
+    "0 & 0 \\\\\n",
+    "0 & 0 \\\\\n",
+    "dt & 0 \\\\\n",
+    "0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})}dt \\\\\n",
+    "\\end{bmatrix}\n",
+    "\\end{equation*}"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "\\begin{equation*}\n",
+    "C = (f(\\bar{z},\\bar{u})-A'\\bar{z}-B'\\bar{u})dt\\\\\n",
+    "= dt(\n",
+    "\\begin{bmatrix} \n",
+    "\\bar{v}cos(\\bar{\\phi})\\\\\n",
+    "\\bar{v}sin(\\bar{\\phi}) \\\\\n",
+    "\\bar{a}\\\\\n",
+    "\\frac{\\bar{v}tan(\\bar{\\delta})}{L}\\\\\n",
+    "\\end{bmatrix}\n",
+    "-\n",
+    "\\begin{bmatrix} \n",
+    "\\bar{v}cos(\\bar{\\phi})-\\bar{v}sin(\\bar{\\phi})\\bar{\\phi}\\\\\n",
+    "\\bar{v}sin(\\bar{\\phi})+\\bar{v}cos(\\bar{\\phi})\\bar{\\phi}\\\\\n",
+    "0\\\\\n",
+    "\\frac{\\bar{v}tan(\\bar{\\delta})}{L}\\\\\n",
+    "\\end{bmatrix}\n",
+    "-\n",
+    "\\begin{bmatrix} \n",
+    "0\\\\\n",
+    "0 \\\\\n",
+    "\\bar{a}\\\\\n",
+    "\\frac{\\bar{v}\\bar{\\delta}}{Lcos^2(\\bar{\\delta})}\\\\\n",
+    "\\end{bmatrix}\n",
+    ")\\\\\n",
+    "=\n",
+    "\\begin{bmatrix} \n",
+    "\\bar{v}sin(\\bar{\\phi})\\bar{\\phi}dt\\\\\n",
+    "-\\bar{v}cos(\\bar{\\phi})\\bar{\\phi}dt\\\\\n",
+    "0\\\\\n",
+    "\\frac{\\bar{v}\\bar{\\delta}}{Lcos^2(\\bar{\\delta})}dt\\\\\n",
+    "\\end{bmatrix}\n",
+    "\\end{equation*}"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "This equation is implemented at \n",
+    "\n",
+    "https://github.com/AtsushiSakai/PythonRobotics/blob/eb6d1cbe6fc90c7be9210bf153b3a04f177cc138/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py#L80-L102"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Reference\n",
+    "\n",
+    "- [Vehicle Dynamics and Control \\| Rajesh Rajamani \\| Springer](http://www.springer.com/us/book/9781461414322)\n",
+    "\n",
+    "- [MPC Course Material \\- MPC Lab @ UC\\-Berkeley](http://www.mpc.berkeley.edu/mpc-course-material)\n"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.6.6"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}

PathTracking/move_to_pose/move_to_pose.py

@@ -1,32 +1,26 @@
 """
+
 Move to specified pose
+
 Author: Daniel Ingram (daniel-s-ingram)
+        Atsushi Sakai(@Atsushi_twi)
 
 P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7
-"""
 
-from __future__ import print_function, division
+"""
 
 import matplotlib.pyplot as plt
 import numpy as np
-
-from math import cos, sin, sqrt, atan2, pi
 from random import random
 
+# simulation parameters
 Kp_rho = 9
 Kp_alpha = 15
 Kp_beta = -3
 dt = 0.01
 
-#Corners of triangular vehicle when pointing to the right (0 radians)
-p1_i = np.array([0.5, 0, 1]).T
-p2_i = np.array([-0.5, 0.25, 1]).T
-p3_i = np.array([-0.5, -0.25, 1]).T
+show_animation = True
 
-x_traj = []
-y_traj = []
-
-plt.ion()
 
 def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
     """
@@ -35,7 +29,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
     beta is the angle between the robot's position and the goal position plus the goal angle
 
     Kp_rho*rho and Kp_alpha*alpha drive the robot along a line towards the goal
-    Kp*beta*beta rotates the line so that it is parallel to the goal angle
+    Kp_beta*beta rotates the line so that it is parallel to the goal angle
     """
     x = x_start
     y = y_start
@@ -44,7 +38,9 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
     x_diff = x_goal - x
     y_diff = y_goal - y
 
-    rho = sqrt(x_diff**2 + y_diff**2)
+    x_traj, y_traj = [], []
+
+    rho = np.sqrt(x_diff**2 + y_diff**2)
     while rho > 0.001:
         x_traj.append(x)
         y_traj.append(y)
@@ -54,63 +50,80 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
 
         """
         Restrict alpha and beta (angle differences) to the range
-        [-pi, pi] to prevent unstable behavior e.g. difference going 
+        [-pi, pi] to prevent unstable behavior e.g. difference going
         from 0 rad to 2*pi rad with slight turn
-        """ 
+        """
 
-        rho = sqrt(x_diff**2 + y_diff**2)
-        alpha = (atan2(y_diff, x_diff) - theta + pi)%(2*pi) - pi
-        beta = (theta_goal - theta - alpha + pi)%(2*pi) - pi
+        rho = np.sqrt(x_diff**2 + y_diff**2)
+        alpha = (np.arctan2(y_diff, x_diff) -
+                 theta + np.pi) % (2 * np.pi) - np.pi
+        beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi
 
-        v = Kp_rho*rho
-        w = Kp_alpha*alpha + Kp_beta*beta
+        v = Kp_rho * rho
+        w = Kp_alpha * alpha + Kp_beta * beta
 
-        if alpha > pi/2 or alpha < -pi/2: 
+        if alpha > np.pi / 2 or alpha < -np.pi / 2:
             v = -v
 
-        theta = theta + w*dt
-        x = x + v*cos(theta)*dt
-        y = y + v*sin(theta)*dt
+        theta = theta + w * dt
+        x = x + v * np.cos(theta) * dt
+        y = y + v * np.sin(theta) * dt
 
-        plot_vehicle(x, y, theta, x_traj, y_traj)
+        if show_animation:
+            plt.cla()
+            plt.arrow(x_start, y_start, np.cos(theta_start),
+                      np.sin(theta_start), color='r', width=0.1)
+            plt.arrow(x_goal, y_goal, np.cos(theta_goal),
+                      np.sin(theta_goal), color='g', width=0.1)
+            plot_vehicle(x, y, theta, x_traj, y_traj)
 
 
 def plot_vehicle(x, y, theta, x_traj, y_traj):
+    # Corners of triangular vehicle when pointing to the right (0 radians)
+    p1_i = np.array([0.5, 0, 1]).T
+    p2_i = np.array([-0.5, 0.25, 1]).T
+    p3_i = np.array([-0.5, -0.25, 1]).T
+
     T = transformation_matrix(x, y, theta)
     p1 = np.matmul(T, p1_i)
     p2 = np.matmul(T, p2_i)
     p3 = np.matmul(T, p3_i)
 
-    plt.cla()
-
     plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'k-')
     plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'k-')
     plt.plot([p3[0], p1[0]], [p3[1], p1[1]], 'k-')
 
-    plt.arrow(x_start, y_start, cos(theta_start), sin(theta_start), color='r', width=0.1)
-    plt.arrow(x_goal, y_goal, cos(theta_goal), sin(theta_goal), color='g', width=0.1)
     plt.plot(x_traj, y_traj, 'b--')
 
     plt.xlim(0, 20)
     plt.ylim(0, 20)
 
-    plt.show()
     plt.pause(dt)
 
+
 def transformation_matrix(x, y, theta):
     return np.array([
-        [cos(theta), -sin(theta), x],
-        [sin(theta), cos(theta), y],
+        [np.cos(theta), -np.sin(theta), x],
+        [np.sin(theta), np.cos(theta), y],
         [0, 0, 1]
-        ])
+    ])
+
+
+def main():
+
+    for i in range(5):
+        x_start = 20 * random()
+        y_start = 20 * random()
+        theta_start = 2 * np.pi * random() - np.pi
+        x_goal = 20 * random()
+        y_goal = 20 * random()
+        theta_goal = 2 * np.pi * random() - np.pi
+        print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" %
+              (x_start, y_start, theta_start))
+        print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" %
+              (x_goal, y_goal, theta_goal))
+        move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
+
 
 if __name__ == '__main__':
-    x_start = 20*random()
-    y_start = 20*random()
-    theta_start = 2*pi*random() - pi
-    x_goal = 20*random()
-    y_goal = 20*random()
-    theta_goal = 2*pi*random() - pi
-    print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" % (x_start, y_start, theta_start))
-    print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" % (x_goal, y_goal, theta_goal))
-    move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
+    main()

PathTracking/rear_wheel_feedback/rear_wheel_feedback.py

@@ -83,12 +83,14 @@ def calc_nearest_index(state, cx, cy, cyaw):
     dx = [state.x - icx for icx in cx]
     dy = [state.y - icy for icy in cy]
 
-    d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
+    d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
 
     mind = min(d)
 
     ind = d.index(mind)
 
+    mind = math.sqrt(mind)
+
     dxl = cx[ind] - state.x
     dyl = cy[ind] - state.y
 

PathTracking/stanley_controller/stanley_controller.py

@@ -5,99 +5,144 @@ Path tracking simulation with Stanley steering control and PID speed control.
 author: Atsushi Sakai (@Atsushi_twi)
 
 """
+from __future__ import division, print_function
+
 import sys
+
 sys.path.append("../../PathPlanning/CubicSpline/")
 
-import math
 import matplotlib.pyplot as plt
-import cubic_spline_planner
+import numpy as np
 
+import cubic_spline_planner
 
 k = 0.5  # control gain
 Kp = 1.0  # speed propotional gain
 dt = 0.1  # [s] time difference
 L = 2.9  # [m] Wheel base of vehicle
-max_steer = math.radians(30.0)  # [rad] max steering angle
+max_steer = np.radians(30.0)  # [rad] max steering angle
 
 show_animation = True
 
 
-class State:
+class State(object):
+    """
+    Class representing the state of a vehicle.
+
+    :param x: (float) x-coordinate
+    :param y: (float) y-coordinate
+    :param yaw: (float) yaw angle
+    :param v: (float) speed
+    """
 
     def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
+        """Instantiate the object."""
+        super(State, self).__init__()
         self.x = x
         self.y = y
         self.yaw = yaw
         self.v = v
 
+    def update(self, acceleration, delta):
+        """
+        Update the state of the vehicle.
 
-def update(state, a, delta):
+        Stanley Control uses bicycle model.
 
-    if delta >= max_steer:
-        delta = max_steer
-    elif delta <= -max_steer:
-        delta = -max_steer
+        :param acceleration: (float) Acceleration
+        :param delta: (float) Steering
+        """
+        delta = np.clip(delta, -max_steer, max_steer)
 
-    state.x = state.x + state.v * math.cos(state.yaw) * dt
-    state.y = state.y + state.v * math.sin(state.yaw) * dt
-    state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
-    state.yaw = pi_2_pi(state.yaw)
-    state.v = state.v + a * dt
-
-    return state
+        self.x += self.v * np.cos(self.yaw) * dt
+        self.y += self.v * np.sin(self.yaw) * dt
+        self.yaw += self.v / L * np.tan(delta) * dt
+        self.yaw = normalize_angle(self.yaw)
+        self.v += acceleration * dt
 
 
-def PIDControl(target, current):
-    a = Kp * (target - current)
+def pid_control(target, current):
+    """
+    Proportional control for the speed.
 
-    return a
+    :param target: (float)
+    :param current: (float)
+    :return: (float)
+    """
+    return Kp * (target - current)
 
 
-def stanley_control(state, cx, cy, cyaw, pind):
+def stanley_control(state, cx, cy, cyaw, last_target_idx):
+    """
+    Stanley steering control.
 
-    ind, efa = calc_target_index(state, cx, cy)
+    :param state: (State object)
+    :param cx: ([float])
+    :param cy: ([float])
+    :param cyaw: ([float])
+    :param last_target_idx: (int)
+    :return: (float, int)
+    """
+    current_target_idx, error_front_axle = calc_target_index(state, cx, cy)
 
-    if pind >= ind:
-        ind = pind
+    if last_target_idx >= current_target_idx:
+        current_target_idx = last_target_idx
 
-    theta_e = pi_2_pi(cyaw[ind] - state.yaw)
-    theta_d = math.atan2(k * efa, state.v)
+    # theta_e corrects the heading error
+    theta_e = normalize_angle(cyaw[current_target_idx] - state.yaw)
+    # theta_d corrects the cross track error
+    theta_d = np.arctan2(k * error_front_axle, state.v)
+    # Steering control
     delta = theta_e + theta_d
 
-    return delta, ind
+    return delta, current_target_idx
 
 
-def pi_2_pi(angle):
-    while (angle > math.pi):
-        angle = angle - 2.0 * math.pi
+def normalize_angle(angle):
+    """
+    Normalize an angle to [-pi, pi].
 
-    while (angle < -math.pi):
-        angle = angle + 2.0 * math.pi
+    :param angle: (float)
+    :return: (float) Angle in radian in [-pi, pi]
+    """
+    while angle > np.pi:
+        angle -= 2.0 * np.pi
+
+    while angle < -np.pi:
+        angle += 2.0 * np.pi
 
     return angle
 
 
 def calc_target_index(state, cx, cy):
+    """
+    Compute index in the trajectory list of the target.
 
-    # calc frant axle position
-    fx = state.x + L * math.cos(state.yaw)
-    fy = state.y + L * math.sin(state.yaw)
+    :param state: (State object)
+    :param cx: [float]
+    :param cy: [float]
+    :return: (int, float)
+    """
+    # Calc front axle position
+    fx = state.x + L * np.cos(state.yaw)
+    fy = state.y + L * np.sin(state.yaw)
 
-    # search nearest point index
+    # Search nearest point index
     dx = [fx - icx for icx in cx]
     dy = [fy - icy for icy in cy]
-    d = [math.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)]
-    mind = min(d)
-    ind = d.index(mind)
+    d = [np.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)]
+    error_front_axle = min(d)
+    target_idx = d.index(error_front_axle)
 
-    tyaw = pi_2_pi(math.atan2(fy - cy[ind], fx - cx[ind]) - state.yaw)
-    if tyaw > 0.0:
-        mind = - mind
+    target_yaw = normalize_angle(np.arctan2(fy - cy[target_idx], fx - cx[target_idx]) - state.yaw)
+    if target_yaw > 0.0:
+        error_front_axle = - error_front_axle
 
-    return ind, mind
+    return target_idx, error_front_axle
 
 
 def main():
+    """Plot an example of Stanley steering control on a cubic spline."""
     #  target course
     ax = [0.0, 100.0, 100.0, 50.0, 60.0]
     ay = [0.0, 0.0, -30.0, -20.0, 0.0]
@@ -107,26 +152,26 @@ def main():
 
     target_speed = 30.0 / 3.6  # [m/s]
 
-    T = 100.0  # max simulation time
+    max_simulation_time = 100.0
 
-    # initial state
-    state = State(x=-0.0, y=5.0, yaw=math.radians(20.0), v=0.0)
+    # Initial state
+    state = State(x=-0.0, y=5.0, yaw=np.radians(20.0), v=0.0)
 
-    lastIndex = len(cx) - 1
+    last_idx = len(cx) - 1
     time = 0.0
     x = [state.x]
     y = [state.y]
     yaw = [state.yaw]
     v = [state.v]
     t = [0.0]
-    target_ind, mind = calc_target_index(state, cx, cy)
+    target_idx, _ = calc_target_index(state, cx, cy)
 
-    while T >= time and lastIndex > target_ind:
-        ai = PIDControl(target_speed, state.v)
-        di, target_ind = stanley_control(state, cx, cy, cyaw, target_ind)
-        state = update(state, ai, di)
+    while max_simulation_time >= time and last_idx > target_idx:
+        ai = pid_control(target_speed, state.v)
+        di, target_idx = stanley_control(state, cx, cy, cyaw, target_idx)
+        state.update(ai, di)
 
-        time = time + dt
+        time += dt
 
         x.append(state.x)
         y.append(state.y)
@@ -138,14 +183,14 @@ def main():
             plt.cla()
             plt.plot(cx, cy, ".r", label="course")
             plt.plot(x, y, "-b", label="trajectory")
-            plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
+            plt.plot(cx[target_idx], cy[target_idx], "xg", label="target")
             plt.axis("equal")
             plt.grid(True)
             plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
             plt.pause(0.001)
 
     # Test
-    assert lastIndex >= target_ind, "Cannot goal"
+    assert last_idx >= target_idx, "Cannot reach goal"
 
     if show_animation:
         plt.plot(cx, cy, ".r", label="course")

README.md

@@ -2,61 +2,78 @@
 
 # PythonRobotics
 [![Build Status](https://travis-ci.org/AtsushiSakai/PythonRobotics.svg?branch=master)](https://travis-ci.org/AtsushiSakai/PythonRobotics)
+[![Documentation Status](https://readthedocs.org/projects/pythonrobotics/badge/?version=latest)](https://pythonrobotics.readthedocs.io/en/latest/?badge=latest)
+[![Say Thanks!](https://img.shields.io/badge/Say%20Thanks-!-1EAEDB.svg)](https://saythanks.io/to/AtsushiSakai)
 
 Python codes for robotics algorithm.
 
+
 # Table of Contents
+   * [What is this?](#what-is-this)
    * [Requirements](#requirements)
+   * [Documentation](#documentation)
    * [How to use](#how-to-use)
    * [Localization](#localization)
       * [Extended Kalman Filter localization](#extended-kalman-filter-localization)
-      * [Unscented Kalman Filter localization](#unscented-kalman-filter-localization)
-      * [Particle Filter localization](#particle-filter-localization)
+      * [Particle filter localization](#particle-filter-localization)
+      * [Histogram filter localization](#histogram-filter-localization)
    * [Mapping](#mapping)
       * [Gaussian grid map](#gaussian-grid-map)
       * [Ray casting grid map](#ray-casting-grid-map)
+      * [k-means object clustering](#k-means-object-clustering)
    * [SLAM](#slam)
       * [Iterative Closest Point (ICP) Matching](#iterative-closest-point-icp-matching)
-      * [EKF SLAM](#ekf-slam)
+      * [FastSLAM 1.0](#fastslam-10)
+      * [Graph based SLAM](#graph-based-slam)
    * [Path Planning](#path-planning)
       * [Dynamic Window Approach](#dynamic-window-approach)
       * [Grid based search](#grid-based-search)
          * [Dijkstra algorithm](#dijkstra-algorithm)
          * [A* algorithm](#a-algorithm)
          * [Potential Field algorithm](#potential-field-algorithm)
-      * [Model Predictive Trajectory Generator](#model-predictive-trajectory-generator)
-         * [Path optimization sample](#path-optimization-sample)
-         * [Lookup table generation sample](#lookup-table-generation-sample)
       * [State Lattice Planning](#state-lattice-planning)
-         * [Uniform polar sampling](#uniform-polar-sampling)
          * [Biased polar sampling](#biased-polar-sampling)
          * [Lane sampling](#lane-sampling)
       * [Probabilistic Road-Map (PRM) planning](#probabilistic-road-map-prm-planning)
-      * [Voronoi Road-Map planning](#voronoi-road-map-planning)
       * [Rapidly-Exploring Random Trees (RRT)](#rapidly-exploring-random-trees-rrt)
-         * [Basic RRT](#basic-rrt)
          * [RRT*](#rrt)
-         * [RRT with dubins path](#rrt-with-dubins-path)
-         * [RRT* with dubins path](#rrt-with-dubins-path-1)
          * [RRT* with reeds-sheep path](#rrt-with-reeds-sheep-path)
-         * [Closed Loop RRT*](#closed-loop-rrt)
-      * [Cubic spline planning](#cubic-spline-planning)
-      * [B-Spline planning](#b-spline-planning)
-      * [Bezier path planning](#bezier-path-planning)
+         * [LQR-RRT*](#lqr-rrt)
       * [Quintic polynomials planning](#quintic-polynomials-planning)
-      * [Dubins path planning](#dubins-path-planning)
       * [Reeds Shepp planning](#reeds-shepp-planning)
+      * [LQR based path planning](#lqr-based-path-planning)
       * [Optimal Trajectory in a Frenet Frame](#optimal-trajectory-in-a-frenet-frame)
    * [Path tracking](#path-tracking)
-      * [Pure pursuit tracking](#pure-pursuit-tracking)
+      * [move to a pose control](#move-to-a-pose-control)
       * [Stanley control](#stanley-control)
       * [Rear wheel feedback control](#rear-wheel-feedback-control)
-      * [Linear–quadratic regulator (LQR) steering control](#linearquadratic-regulator-lqr-steering-control)
       * [Linear–quadratic regulator (LQR) speed and steering control](#linearquadratic-regulator-lqr-speed-and-steering-control)
       * [Model predictive speed and steering control](#model-predictive-speed-and-steering-control)
+   * [Arm Navigation](#arm-navigation)
+      * [N joint arm to point control](#n-joint-arm-to-point-control)
+   * [Aerial Navigation](#aerial-navigation)
+      * [drone 3d trajectory following](#drone-3d-trajectory-following)
    * [License](#license)
    * [Contribution](#contribution)
-   * [Author](#author)
+   * [Support](#support)
+   * [Authors](#authors)
+
+# What is this?
+
+This is a Python code collection of robotics algorithms, especially for autonomous navigation.
+
+Features:
+
+1. Easy to read for understanding each algorithm's basic idea.
+
+2. Widely used and practical algorithms are selected.
+
+3. Minimum dependency.
+
+See this paper for more details:
+
+- [\[1808\.10703\] PythonRobotics: a Python code collection of robotics algorithms](https://arxiv.org/abs/1808.10703) ([BibTeX](https://github.com/AtsushiSakai/PythonRoboticsPaper/blob/master/python_robotics.bib))
+
 
 # Requirements
 
@@ -70,11 +87,17 @@ Python codes for robotics algorithm.
 
 - pandas
 
-- [cvxpy 0.4.x](http://www.cvxpy.org/en/latest/) 
+- [cvxpy](http://www.cvxpy.org/en/latest/) 
+
+# Documentation
+
+This README only shows some examples of this project. 
+
+Full documentation is available online: [https://pythonrobotics.readthedocs.io/](https://pythonrobotics.readthedocs.io/)
 
 # How to use
 
-1. Install the required libraries.
+1. Install the required libraries. You can use environment.yml with conda command.
 
 2. Clone this repo.
 
@@ -96,19 +119,11 @@ the green point is positioning observation (ex. GPS), and the red line is estima
 
 The red ellipse is estimated covariance ellipse with EKF.
 
-## Unscented Kalman Filter localization
-
-![2](https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/unscented_kalman_filter/animation.gif)
-
-This is a sensor fusion localization with Unscented Kalman Filter(UKF).
-
-The lines and points are same meaning of the EKF simulation.
-
 Ref:
 
-- [Discriminatively Trained Unscented Kalman Filter for Mobile Robot Localization](https://www.researchgate.net/publication/267963417_Discriminatively_Trained_Unscented_Kalman_Filter_for_Mobile_Robot_Localization)
+- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
 
-## Particle Filter localization
+## Particle filter localization
 
 ![2](https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/particle_filter/animation.gif)
 
@@ -122,11 +137,36 @@ It is assumed that the robot can measure a distance from landmarks (RFID).
 
 This measurements are used for PF localization.
 
+Ref:
+
+- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
+
+
+## Histogram filter localization
+
+![3](https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/histogram_filter/animation.gif)
+
+This is a 2D localization example with Histogram filter.
+
+The red cross is true position, black points are RFID positions.
+
+The blue grid shows a position probability of histogram filter.  
+
+In this simulation, x,y are unknown, yaw is known.
+
+The filter integrates speed input and range observations from RFID for localization.
+
+Initial position is not needed.
+
+Ref:
+
+- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
+
 # Mapping
 
 ## Gaussian grid map
 
-This is a 2D gaussian grid mapping example.
+This is a 2D Gaussian grid mapping example.
 
 ![2](https://github.com/AtsushiSakai/PythonRobotics/raw/master/Mapping/gaussian_grid_map/animation.gif)
 
@@ -136,6 +176,11 @@ This is a 2D ray casting grid mapping example.
 
 ![2](https://github.com/AtsushiSakai/PythonRobotics/raw/master/Mapping/raycasting_grid_map/animation.gif)
 
+## k-means object clustering
+
+This is a 2D object clustering with k-means algorithm.
+
+![2](https://github.com/AtsushiSakai/PythonRobotics/raw/master/Mapping/kmeans_clustering/animation.gif)
 
 # SLAM
 
@@ -154,20 +199,46 @@ Ref:
 - [Introduction to Mobile Robotics: Iterative Closest Point Algorithm](https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf)
 
 
-## EKF SLAM
+## FastSLAM 1.0
 
-This is a Extended Kalman Filter based SLAM example.
+This is a feature based SLAM example using FastSLAM 1.0.
 
-The blue line is ground truth, the black line is dead reckoning, the red line is the estimated trajectory with EKF SLAM.
+The blue line is ground truth, the black line is dead reckoning, the red line is the estimated trajectory with FastSLAM.
 
-The green cross are estimated landmarks.
+The red points are particles of FastSLAM.
+
+Black points are landmarks, blue crosses are estimated landmark positions by FastSLAM.
+
+
+![3](https://github.com/AtsushiSakai/PythonRobotics/raw/master/SLAM/FastSLAM1/animation.gif)
 
-![3](https://github.com/AtsushiSakai/PythonRobotics/raw/master/SLAM/EKFSLAM/animation.gif)
 
 Ref:
 
 - [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)
 
+- [SLAM simulations by Tim Bailey](http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm)
+
+
+## Graph based SLAM
+
+This is a graph based SLAM example.
+
+The blue line is ground truth.
+
+The black line is dead reckoning.
+
+The red line is the estimated trajectory with Graph based SLAM.
+
+The black stars are landmarks for graph edge generation.
+
+![3](https://github.com/AtsushiSakai/PythonRobotics/raw/master/SLAM/GraphBasedSLAM/animation.gif)
+
+Ref:
+
+- [A Tutorial on Graph-Based SLAM](http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf)
+
+
 # Path Planning
 
 ## Dynamic Window Approach
@@ -197,7 +268,7 @@ This is a 2D grid based shortest path planning with A star algorithm.
 
 In the animation, cyan points are searched nodes.
 
-It's heuristic is 2D Euclid distance.
+Its heuristic is 2D Euclid distance.
 
 ### Potential Field algorithm
 
@@ -211,36 +282,17 @@ Ref:
 
 - [Robotic Motion Planning:Potential Functions](https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf)
 
-
-## Model Predictive Trajectory Generator
-
-This is a path optimization sample on model predictive trajectory generator.
-
-This algorithm is used for state lattice planner. 
-
-### Path optimization sample
-
-![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/ModelPredictiveTrajectoryGenerator/kn05animation.gif)
-
-### Lookup table generation sample
-
-![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png?raw=True)
-
-Ref: 
-
-- [Optimal rough terrain trajectory generation for wheeled mobile robots](http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328)
-
-
 ## State Lattice Planning
 
 This script is a path planning code with state lattice planning.
 
 This code uses the model predictive trajectory generator to solve boundary problem.
 
+Ref: 
 
-### Uniform polar sampling
+- [Optimal rough terrain trajectory generation for wheeled mobile robots](http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328)
 
-![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/StateLatticePlanner/UniformPolarSampling.gif)
+- [State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments](http://www.frc.ri.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf)
 
 
 ### Biased polar sampling
@@ -269,38 +321,14 @@ Ref:
 - [Probabilistic roadmap \- Wikipedia](https://en.wikipedia.org/wiki/Probabilistic_roadmap)
 
 　　
-## Voronoi Road-Map planning 
-
-![VRM](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/VoronoiRoadMap/animation.gif)
-
-This Voronoi road-map planner uses Dijkstra method for graph search.
-
-In the animation, blue points are Voronoi points,
-
-Cyan crosses means searched points with Dijkstra method,
-
-The red line is the final path of Vornoi Road-Map.
-
-Ref:
-
-- [Robotic Motion Planning](https://www.cs.cmu.edu/~motionplanning/lecture/Chap5-RoadMap-Methods_howie.pdf)
-
 
 ## Rapidly-Exploring Random Trees (RRT)
 
-### Basic RRT 
-
-![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/RRT/animation.gif)
-
-This script is a simple path planning code with Rapidly-Exploring Random Trees (RRT)
-
-Black circles are obstacles, green line is a searched tree, red crosses are start and goal positions.
-
 ### RRT\*
 
 ![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/RRTstar/animation.gif)
 
-This script is a  path planning code with RRT\*
+This is a path planning code with RRT\*
 
 Black circles are obstacles, green line is a searched tree, red crosses are start and goal positions.
 
@@ -310,92 +338,26 @@ Ref:
 
 - [Sampling-based Algorithms for Optimal Motion Planning](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf)
 
-
-### RRT with dubins path 
-
-![PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/RRTDubins/animation.gif)
-
-Path planning for a car robot with RRT and dubins path planner.
-
-
-### RRT\* with dubins path
-
-![AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/RRTStarDubins/animation.gif)
-
-Path planning for a car robot with RRT\* and dubins path planner.
-
-
 ### RRT\* with reeds-sheep path
 
 ![Robotics/animation.gif at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/RRTStarReedsShepp/animation.gif))
 
 Path planning for a car robot with RRT\* and reeds sheep path planner.
 
-### Closed Loop RRT\*
+### LQR-RRT\*
 
-A vehicle model based path planning with closed loop RRT\*.
+This is a path planning simulation with LQR-RRT\*.
 
-![CLRRT](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/ClosedLoopRRTStar/animation.gif)
+A double integrator motion model is used for LQR local planner.
 
-In this code, pure-pursuit algorithm is used for steering control, 
-
-PID is used for speed control.
+![LQRRRT](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/LQRRRTStar/animation.gif)
 
 Ref:
 
-- [Motion Planning in Complex Environments
-using Closed-loop Prediction](http://acl.mit.edu/papers/KuwataGNC08.pdf)
+- [LQR\-RRT\*: Optimal Sampling\-Based Motion Planning with Automatically Derived Extension Heuristics](http://lis.csail.mit.edu/pubs/perez-icra12.pdf)
 
-- [Real-time Motion Planning with Applications to
-Autonomous Urban Driving](http://acl.mit.edu/papers/KuwataTCST09.pdf)
+- [MahanFathi/LQR\-RRTstar: LQR\-RRT\* method is used for random motion planning of a simple pendulum in its phase plot](https://github.com/MahanFathi/LQR-RRTstar)
 
-- [[1601.06326] Sampling-based Algorithms for Optimal Motion Planning Using Closed-loop Prediction](https://arxiv.org/abs/1601.06326)
-
-## Cubic spline planning
-
-A sample code for cubic path planning.
-
-This code generates a curvature continuous path based on x-y waypoints with cubic spline.
-
-Heading angle of each point can be also calculated analytically.
-
-![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/CubicSpline/Figure_1.png?raw=True)
-![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/CubicSpline/Figure_2.png?raw=True)
-![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/CubicSpline/Figure_3.png?raw=True)
-
-
-## B-Spline planning
-
-![B-Spline](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/BSplinePath/Figure_1.png?raw=True)
-
-This is a path planning with B-Spline curse.
-
-If you input waypoints, it generates a smooth path with B-Spline curve.
-
-The final course should be on the first and last waypoints. 
-
-Ref: 
-
-- [B\-spline \- Wikipedia](https://en.wikipedia.org/wiki/B-spline)
-
-## Bezier path planning
-
-A sample code of Bezier path planning.
-
-It is based on 4 control points Beier path.
-
-![Bezier1](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/BezierPath/Figure_1.png?raw=True)
-
-If you change the offset distance from start and end point,
-
-You can get different Beizer course:
-
-![Bezier2](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/BezierPath/Figure_2.png?raw=True)
-
- 
-Ref:
-
-- [Continuous Curvature Path Generation Based on Bezier Curves for Autonomous Vehicles](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.294.6438&rep=rep1&type=pdf)
 
 ## Quintic polynomials planning
 
@@ -409,17 +371,6 @@ Ref:
 
 - [Local Path Planning And Motion Control For Agv In Positioning](http://ieeexplore.ieee.org/document/637936/)
 
-
-## Dubins path planning
-
-A sample code for Dubins path planning.
-
-![dubins](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/DubinsPath/animation.gif?raw=True)
-
-Ref:
-
-- [Dubins path - Wikipedia](https://en.wikipedia.org/wiki/Dubins_path)
-
 ## Reeds Shepp planning
 
 A sample code with Reeds Shepp path planning.
@@ -435,6 +386,13 @@ Ref:
 - [ghliu/pyReedsShepp: Implementation of Reeds Shepp curve\.](https://github.com/ghliu/pyReedsShepp)
 
 
+## LQR based path planning
+
+A sample code using LQR based path planning for double integrator model.
+
+![RSPlanning](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/LQRPlanner/animation.gif?raw=true)
+
+
 ## Optimal Trajectory in a Frenet Frame 
 
 ![3](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/FrenetOptimalTrajectory/animation.gif)
@@ -454,17 +412,16 @@ Ref:
 
 # Path tracking
 
-## Pure pursuit tracking
+## move to a pose control
 
-Path tracking simulation with pure pursuit steering control and PID speed control.
+This is a simulation of moving to a pose control
 
-![2](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/pure_pursuit/animation.gif)
-
-The red line is a target course, the green cross means the target point for pure pursuit control, the blue line is the tracking.
+![2](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/move_to_pose/animation.gif)
 
 Ref:
 
-- [A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles](https://arxiv.org/abs/1604.07446)
+- [P. I. Corke, "Robotics, Vision and Control" \| SpringerLink p102](https://link.springer.com/book/10.1007/978-3-642-20144-8)
+
 
 ## Stanley control
 
@@ -491,17 +448,6 @@ Ref:
 - [A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles](https://arxiv.org/abs/1604.07446)
 
 
-## Linear–quadratic regulator (LQR) steering control
-
-Path tracking simulation with LQR steering control and PID speed control.
-
-![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/lqr_steer_control/animation.gif)
-
-Ref:
-
-- [ApolloAuto/apollo: An open autonomous driving platform](https://github.com/ApolloAuto/apollo)
-
-
 ## Linear–quadratic regulator (LQR) speed and steering control
 
 Path tracking simulation with LQR speed and steering control.
@@ -519,17 +465,33 @@ Path tracking simulation with iterative linear model predictive speed and steeri
 
 <img src="https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/model_predictive_speed_and_steer_control/animation.gif" width="640">
 
-This code uses cvxpy as an optimization modeling tool.
-
-- [Welcome to CVXPY](http://www.cvxpy.org/en/latest/)
-
 Ref:
 
-- [Vehicle Dynamics and Control \| Rajesh Rajamani \| Springer](http://www.springer.com/us/book/9781461414322)
+- [notebook](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/model_predictive_speed_and_steer_control/notebook.ipynb)
 
-- [MPC Course Material \- MPC Lab @ UC\-Berkeley](http://www.mpc.berkeley.edu/mpc-course-material)
 
- 
+# Arm Navigation
+
+## N joint arm to point control
+
+N joint arm to a point control simulation.
+
+This is a interactive simulation.
+
+You can set the goal position of the end effector with left-click on the ploting area. 
+
+![3](https://github.com/AtsushiSakai/PythonRobotics/raw/master/ArmNavigation/n_joint_arm_to_point_control/animation.gif)
+
+In this simulation N = 10, however, you can change it.
+
+# Aerial Navigation
+
+## drone 3d trajectory following 
+
+This is a 3d trajectory following simulation for a quadrotor.
+
+![3](https://github.com/AtsushiSakai/PythonRobotics/raw/master/AerialNavigation/drone_3d_trajectory_following/animation.gif)
+
 
 # License 
 
@@ -537,14 +499,41 @@ MIT
 
 # Contribution
 
-If you have any quesions or problems about this code, feel free to add an issue.
-
 A small PR like bug fix is welcome.
 
-# Author
-
-Atsushi Sakai ([@Atsushi_twi](https://twitter.com/Atsushi_twi))
+If your PR is merged multiple times, I will add your account to the author list.
 
+# Support
+
+If you or your company would like to support this project, please consider:
+
+- [Become a backer or sponsor on Patreon](https://www.patreon.com/myenigma)
+
+- [One-time donation via PayPal](https://www.paypal.me/myenigmapay/)
+
+You can add your name or your company logo in README if you are a patron.
+
+E-mail consultant is also available.
+
+ 　
+
+Your comment using [![Say Thanks!](https://img.shields.io/badge/Say%20Thanks-!-1EAEDB.svg)](https://saythanks.io/to/AtsushiSakai) is also welcome. 
+
+This is a list: [Users comments](https://github.com/AtsushiSakai/PythonRobotics/blob/master/users_comments.md)
+
+# Authors
+
+- [Atsushi Sakai](https://github.com/AtsushiSakai/) ([@Atsushi_twi](https://twitter.com/Atsushi_twi))
+
+- [Daniel Ingram](https://github.com/daniel-s-ingram)
+
+- [Joe Dinius](https://github.com/jwdinius)
+
+- [Karan Chawla](https://github.com/karanchawla)
+
+- [Antonin RAFFIN](https://github.com/araffin)
+
+- [Alexis Paques](https://github.com/AlexisTM)
 
 
 

RoboticArm/two_joint_arm.py

@@ -1,83 +0,0 @@
-"""
-Inverse kinematics of a two-joint arm
-Left-click the plot to set the goal position of the end effector
-
-Author: Daniel Ingram (daniel-s-ingram)
-"""
-from __future__ import print_function, division
-
-import matplotlib.pyplot as plt
-import numpy as np
-
-from math import sin, cos, atan2, acos, pi
-
-Kp = 15
-dt = 0.01
-
-#Link lengths
-l1 = l2 = 1
-
-shoulder = np.array([0, 0])
-
-#Set initial goal position to the initial end-effector position
-x = 2
-y = 0
-
-plt.ion()
-
-def two_joint_arm():
-    """
-    Computes the inverse kinematics for a planar 2DOF arm
-    """
-    theta1 = theta2 = 0
-    while True:
-        try:
-            theta2_goal = acos((x**2 + y**2 - l1**2 -l2**2)/(2*l1*l2))
-            theta1_goal = atan2(y, x) - atan2(l2*sin(theta2_goal), (l1 + l2*cos(theta2_goal)))
-
-            if theta1_goal < 0:
-                theta2_goal = -theta2_goal
-                theta1_goal = atan2(y, x) - atan2(l2*sin(theta2_goal), (l1 + l2*cos(theta2_goal)))
-
-            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")
-
-        plot_arm(theta1, theta2, x, y)
-
-def plot_arm(theta1, theta2, x, y):
-    elbow = shoulder + np.array([l1*cos(theta1), l1*sin(theta1)])
-    wrist = elbow + np.array([l2*cos(theta1+theta2), l2*sin(theta1+theta2)])
-
-    plt.cla()
-
-    plt.plot([shoulder[0], elbow[0]], [shoulder[1], elbow[1]], 'k-')
-    plt.plot([elbow[0], wrist[0]], [elbow[1], wrist[1]], 'k-')
-
-    plt.plot(shoulder[0], shoulder[1], 'ro')
-    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.xlim(-2, 2)
-    plt.ylim(-2, 2)
-    
-    plt.show()
-    plt.pause(dt)
-
-def ang_diff(theta1, theta2):
-    #Returns the difference between two angles in the range -pi to +pi
-    return (theta1-theta2+pi)%(2*pi)-pi
-
-def click(event):
-    global x, y
-    x = event.xdata
-    y = event.ydata
-
-if __name__ == "__main__":
-    fig = plt.figure()
-    fig.canvas.mpl_connect("button_press_event", click)
-    two_joint_arm()

SLAM/EKFSLAM/ekf_slam.py

@@ -203,13 +203,7 @@ def jacobH(q, delta, x, i):
 
 
 def pi_2_pi(angle):
-    while(angle > math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while(angle < -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
+    return (angle + math.pi) % (2*math.pi) - math.pi
 
 
 def main():

SLAM/FastSLAM1/fast_slam1.py

@@ -1,6 +1,6 @@
 """
 
-Fast SLAM example
+FastSLAM 1.0 example
 
 author: Atsushi Sakai (@Atsushi_twi)
 
@@ -11,12 +11,14 @@ import math
 import matplotlib.pyplot as plt
 
 
-# EKF state covariance
-Cx = np.diag([1.0, 1.0, math.radians(30.0)])**2
+# Fast SLAM covariance
+Q = np.diag([3.0, math.radians(10.0)])**2
+R = np.diag([1.0, math.radians(20.0)])**2
 
 #  Simulation parameter
-Qsim = np.diag([0.0, math.radians(0.0)])**2
-Rsim = np.diag([1.0, math.radians(10.0)])**2
+Qsim = np.diag([0.3, math.radians(2.0)])**2
+Rsim = np.diag([0.5, math.radians(10.0)])**2
+OFFSET_YAWRATE_NOISE = 0.01
 
 DT = 0.1  # time tick [s]
 SIM_TIME = 50.0  # simulation time [s]
@@ -24,10 +26,8 @@ MAX_RANGE = 20.0  # maximum observation range
 M_DIST_TH = 2.0  # Threshold of Mahalanobis distance for data association.
 STATE_SIZE = 3  # State size [x,y,yaw]
 LM_SIZE = 2  # LM srate size [x,y]
-
 N_PARTICLE = 100  # number of particle
-
-NTH = N_PARTICLE / 2.0  # Number of particle for re-sampling
+NTH = N_PARTICLE / 1.5  # Number of particle for re-sampling
 
 show_animation = True
 
@@ -39,22 +39,35 @@ class Particle:
         self.x = 0.0
         self.y = 0.0
         self.yaw = 0.0
-        self.lm = np.zeros((N_LM, 2))
-        self.lmP = [np.zeros((2, 2))] * N_LM
+        # landmark x-y positions
+        self.lm = np.matrix(np.zeros((N_LM, LM_SIZE)))
+        # landmark position covariance
+        self.lmP = np.matrix(np.zeros((N_LM * LM_SIZE, LM_SIZE)))
+
+
+def fast_slam1(particles, u, z):
+
+    particles = predict_particles(particles, u)
+
+    particles = update_with_observation(particles, z)
+
+    particles = resampling(particles)
+
+    return particles
 
 
 def normalize_weight(particles):
 
     sumw = sum([p.w for p in particles])
-    #  print(sumw)
 
-    #  if sumw <= 0.0000001:
-    #  for i in range(N_PARTICLE):
-    #  particles[i].w = 1.0 / N_PARTICLE
-    #  return particles
+    try:
+        for i in range(N_PARTICLE):
+            particles[i].w /= sumw
+    except ZeroDivisionError:
+        for i in range(N_PARTICLE):
+            particles[i].w = 1.0 / N_PARTICLE
 
-    for i in range(N_PARTICLE):
-        particles[i].w = particles[i].w / sumw
+        return particles
 
     return particles
 
@@ -69,7 +82,6 @@ def calc_final_state(particles):
         xEst[0, 0] += particles[i].w * particles[i].x
         xEst[1, 0] += particles[i].w * particles[i].y
         xEst[2, 0] += particles[i].w * particles[i].yaw
-        #  print(particles[i].x, particles[i].y, particles[i].yaw, particles[i].w)
 
     xEst[2, 0] = pi_2_pi(xEst[2, 0])
     #  print(xEst)
@@ -84,7 +96,7 @@ def predict_particles(particles, u):
         px[0, 0] = particles[i].x
         px[1, 0] = particles[i].y
         px[2, 0] = particles[i].yaw
-        ud = u + (np.matrix(np.random.randn(1, 2)) * Rsim).T  # add noise
+        ud = u + (np.matrix(np.random.randn(1, 2)) * R).T  # add noise
         px = motion_model(px, ud)
         particles[i].x = px[0, 0]
         particles[i].y = px[1, 0]
@@ -93,110 +105,102 @@ def predict_particles(particles, u):
     return particles
 
 
-def add_new_lm(particle, z):
+def add_new_lm(particle, z, Q):
 
     r = z[0, 0]
     b = z[0, 1]
     lm_id = int(z[0, 2])
 
-    s = math.sin(particle.yaw + b)
-    c = math.cos(particle.yaw + b)
+    s = math.sin(pi_2_pi(particle.yaw + b))
+    c = math.cos(pi_2_pi(particle.yaw + b))
 
     particle.lm[lm_id, 0] = particle.x + r * c
     particle.lm[lm_id, 1] = particle.y + r * s
-    #  print(particle.lm)
-    #  print(lm_id)
 
     # covariance
     Gz = np.matrix([[c, -r * s],
                     [s, r * c]])
 
-    particle.lmP[lm_id] = Gz * Cx[0:2, 0:2] * Gz.T
+    particle.lmP[2 * lm_id:2 * lm_id + 2] = Gz * Q * Gz.T
 
     return particle
 
 
-def compute_jacobians(particle, xf, Pf, R):
-    dx = xf[0] - particle.x
-    dy = xf[1] - particle.y
+def compute_jacobians(particle, xf, Pf, Q):
+    dx = xf[0, 0] - particle.x
+    dy = xf[1, 0] - particle.y
     d2 = dx**2 + dy**2
     d = math.sqrt(d2)
 
-    zp = np.matrix([[d, pi_2_pi(math.atan2(dy, dx) - particle.yaw)]])
+    zp = np.matrix([[d, pi_2_pi(math.atan2(dy, dx) - particle.yaw)]]).T
 
     Hv = np.matrix([[-dx / d, -dy / d, 0.0],
                     [dy / d2, -dx / d2, -1.0]])
 
-    Hf = np.matrix([[dx / d, -dy / d],
+    Hf = np.matrix([[dx / d, dy / d],
                     [-dy / d2, dx / d2]])
 
-    Sf = Hf * Pf * Hf.T + R
+    Sf = Hf * Pf * Hf.T + Q
 
     return zp, Hv, Hf, Sf
 
 
-def KF_cholesky_update(xf, Pf, v, R, Hf):
+def update_KF_with_cholesky(xf, Pf, v, Q, Hf):
     PHt = Pf * Hf.T
-    S = Hf * PHt + R
+    S = Hf * PHt + Q
 
     S = (S + S.T) * 0.5
     SChol = np.linalg.cholesky(S).T
-
     SCholInv = np.linalg.inv(SChol)
     W1 = PHt * SCholInv
     W = W1 * SCholInv.T
 
-    x = xf + (W * v.T).T
+    x = xf + W * v
     P = Pf - W1 * W1.T
 
     return x, P
 
 
-def feature_update(particle, z, R):
+def update_landmark(particle, z, Q):
 
     lm_id = int(z[0, 2])
-    xf = particle.lm[lm_id, :]
-    Pf = particle.lmP[lm_id]
-    #  print(xf)
-    #  print(particle.lm)
+    xf = np.matrix(particle.lm[lm_id, :]).T
+    Pf = np.matrix(particle.lmP[2 * lm_id:2 * lm_id + 2, :])
 
-    zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, R)
+    zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q)
 
-    v = z[0, 0:2] - zp
-    v[0, 1] = pi_2_pi(v[0, 1])
-    #  print(v)
+    dz = z[0, 0: 2].T - zp
+    dz[1, 0] = pi_2_pi(dz[1, 0])
 
-    xf, Pf = KF_cholesky_update(xf, Pf, v, R, Hf)
+    xf, Pf = update_KF_with_cholesky(xf, Pf, dz, Q, Hf)
 
-    particle.lm[lm_id, :] = xf
-    particle.lmP[lm_id] = Pf
-
-    #  print(xf)
-    #  print(particle.lm)
-    #  print(Pf)
-    #  input()
+    particle.lm[lm_id, :] = xf.T
+    particle.lmP[2 * lm_id:2 * lm_id + 2, :] = Pf
 
     return particle
 
 
-def compute_weight(particle, z, R):
+def compute_weight(particle, z, Q):
 
     lm_id = int(z[0, 2])
-    xf = particle.lm[lm_id, :]
-    Pf = particle.lmP[lm_id]
-    zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, R)
+    xf = np.matrix(particle.lm[lm_id, :]).T
+    Pf = np.matrix(particle.lmP[2 * lm_id:2 * lm_id + 2])
+    zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q)
 
-    dx = z[0, 0:2] - zp
-    dx[0, 1] = pi_2_pi(dx[0, 1])
-    dx = dx.T
+    dx = z[0, 0: 2].T - zp
+    dx[1, 0] = pi_2_pi(dx[1, 0])
 
-    S = particle.lmP[lm_id]
+    S = particle.lmP[2 * lm_id:2 * lm_id + 2]
+    try:
+        invS = np.linalg.inv(S)
+    except np.linalg.linalg.LinAlgError:
+        print("singuler")
+        return 1.0
 
-    num = math.exp(-0.5 * dx.T * np.linalg.inv(S) * dx)
+    num = math.exp(-0.5 * dx.T * invS * dx)
     den = 2.0 * math.pi * math.sqrt(np.linalg.det(S))
 
     w = num / den
-    print(w)
 
     return w
 
@@ -209,15 +213,13 @@ def update_with_observation(particles, z):
 
         for ip in range(N_PARTICLE):
             # new landmark
-            if abs(particles[ip].lm[lmid, 0]) <= 0.1:
-                particles[ip] = add_new_lm(particles[ip], z[iz, :])
+            if abs(particles[ip].lm[lmid, 0]) <= 0.01:
+                particles[ip] = add_new_lm(particles[ip], z[iz, :], Q)
             # known landmark
             else:
-                # w = p(z_k | x_k)
-                w = compute_weight(particles[ip], z[iz, :], Cx[0:2, 0:2])
-                particles[ip].w = particles[ip].w * w
-                particles[ip] = feature_update(
-                    particles[ip], z[iz, :], Cx[0:2, 0:2])
+                w = compute_weight(particles[ip], z[iz, :], Q)
+                particles[ip].w *= w
+                particles[ip] = update_landmark(particles[ip], z[iz, :], Q)
 
     return particles
 
@@ -236,10 +238,9 @@ def resampling(particles):
     pw = np.matrix(pw)
 
     Neff = 1.0 / (pw * pw.T)[0, 0]  # Effective particle number
-    print(Neff)
+    #  print(Neff)
 
     if Neff < NTH:  # resampling
-        print("resamping")
         wcum = np.cumsum(pw)
         base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
         resampleid = base + np.random.rand(base.shape[1]) / N_PARTICLE
@@ -251,41 +252,29 @@ def resampling(particles):
                 ind += 1
             inds.append(ind)
 
-        #  print(inds)
-        #  print(pw)
-
         tparticles = particles[:]
         for i in range(len(inds)):
-            particles[i] = tparticles[inds[i]]
+            particles[i].x = tparticles[inds[i]].x
+            particles[i].y = tparticles[inds[i]].y
+            particles[i].yaw = tparticles[inds[i]].yaw
+            particles[i].lm = tparticles[inds[i]].lm[:, :]
+            particles[i].lmP = tparticles[inds[i]].lmP[:, :]
             particles[i].w = 1.0 / N_PARTICLE
 
-        particles = normalize_weight(particles)
-        #  input()
-
     return particles
 
 
-def fast_slam(particles, PEst, u, z):
+def calc_input(time):
 
-    # Predict
-    particles = predict_particles(particles, u)
+    if time <= 3.0:
+        v = 0.0
+        yawrate = 0.0
+    else:
+        v = 1.0  # [m/s]
+        yawrate = 0.1  # [rad/s]
 
-    # Observation
-    particles = update_with_observation(particles, z)
-
-    particles = normalize_weight(particles)
-
-    particles = resampling(particles)
-
-    xEst = calc_final_state(particles)
-
-    return xEst, PEst
-
-
-def calc_input():
-    v = 1.0  # [m/s]
-    yawrate = 0.1  # [rad/s]
     u = np.matrix([v, yawrate]).T
+
     return u
 
 
@@ -301,16 +290,16 @@ def observation(xTrue, xd, u, RFID):
         dx = RFID[i, 0] - xTrue[0, 0]
         dy = RFID[i, 1] - xTrue[1, 0]
         d = math.sqrt(dx**2 + dy**2)
-        angle = pi_2_pi(math.atan2(dy, dx))
+        angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
         if d <= MAX_RANGE:
             dn = d + np.random.randn() * Qsim[0, 0]  # add noise
             anglen = angle + np.random.randn() * Qsim[1, 1]  # add noise
-            zi = np.matrix([dn, anglen, i])
+            zi = np.matrix([dn, pi_2_pi(anglen), i])
             z = np.vstack((z, zi))
 
     # add noise to input
     ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
-    ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
+    ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1] + OFFSET_YAWRATE_NOISE
     ud = np.matrix([ud1, ud2]).T
 
     xd = motion_model(xd, ud)
@@ -335,36 +324,8 @@ def motion_model(x, u):
     return x
 
 
-def calc_n_LM(x):
-    n = int((len(x) - STATE_SIZE) / LM_SIZE)
-    return n
-
-
-def calc_LM_Pos(x, z):
-
-    zp = np.zeros((2, 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
-
-
-def get_LM_Pos_from_state(x, ind):
-
-    lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :]
-
-    return lm
-
-
 def pi_2_pi(angle):
-    while(angle > math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while(angle < -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
+    return (angle + math.pi) % (2*math.pi) - math.pi
 
 
 def main():
@@ -375,14 +336,18 @@ def main():
     # RFID positions [x, y]
     RFID = np.array([[10.0, -2.0],
                      [15.0, 10.0],
+                     [15.0, 15.0],
+                     [10.0, 20.0],
                      [3.0, 15.0],
-                     [-5.0, 20.0]])
+                     [-5.0, 20.0],
+                     [-5.0, 5.0],
+                     [-10.0, 15.0]
+                     ])
     N_LM = RFID.shape[0]
 
     # State Vector [x y yaw v]'
     xEst = np.matrix(np.zeros((STATE_SIZE, 1)))
     xTrue = np.matrix(np.zeros((STATE_SIZE, 1)))
-    PEst = np.eye(STATE_SIZE)
 
     xDR = np.matrix(np.zeros((STATE_SIZE, 1)))  # Dead reckoning
 
@@ -395,13 +360,15 @@ def main():
 
     while SIM_TIME >= time:
         time += DT
-        u = calc_input()
+        u = calc_input(time)
 
         xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
 
-        xEst, PEst = fast_slam(particles, PEst, ud, z)
+        particles = fast_slam1(particles, ud, z)
 
-        x_state = xEst[0:STATE_SIZE]
+        xEst = calc_final_state(particles)
+
+        x_state = xEst[0: STATE_SIZE]
 
         # store data history
         hxEst = np.hstack((hxEst, x_state))
@@ -410,20 +377,11 @@ def main():
 
         if show_animation:
             plt.cla()
-
             plt.plot(RFID[:, 0], RFID[:, 1], "*k")
 
             for i in range(N_PARTICLE):
                 plt.plot(particles[i].x, particles[i].y, ".r")
-
-                #  for ii in range(N_LM):
-                #  plt.plot(particles[i].lm[ii, 0],
-                #  particles[i].lm[ii, 1], "xb")
-
-            # plot landmark
-            for i in range(calc_n_LM(xEst)):
-                plt.plot(xEst[STATE_SIZE + i * 2],
-                         xEst[STATE_SIZE + i * 2 + 1], "xg")
+                plt.plot(particles[i].lm[:, 0], particles[i].lm[:, 1], "xb")
 
             plt.plot(np.array(hxTrue[0, :]).flatten(),
                      np.array(hxTrue[1, :]).flatten(), "-b")

SLAM/FastSLAM2/fast_slam2.py

@@ -0,0 +1,434 @@
+"""
+
+FastSLAM 2.0 example
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+"""
+
+import numpy as np
+import math
+import matplotlib.pyplot as plt
+
+
+# Fast SLAM covariance
+Q = np.diag([3.0, math.radians(10.0)])**2
+R = np.diag([1.0, math.radians(20.0)])**2
+
+#  Simulation parameter
+Qsim = np.diag([0.3, math.radians(2.0)])**2
+Rsim = np.diag([0.5, math.radians(10.0)])**2
+OFFSET_YAWRATE_NOISE = 0.01
+
+DT = 0.1  # time tick [s]
+SIM_TIME = 50.0  # simulation time [s]
+MAX_RANGE = 20.0  # maximum observation range
+M_DIST_TH = 2.0  # Threshold of Mahalanobis distance for data association.
+STATE_SIZE = 3  # State size [x,y,yaw]
+LM_SIZE = 2  # LM srate size [x,y]
+N_PARTICLE = 100  # number of particle
+NTH = N_PARTICLE / 1.0  # Number of particle for re-sampling
+
+show_animation = True
+
+
+class Particle:
+
+    def __init__(self, N_LM):
+        self.w = 1.0 / N_PARTICLE
+        self.x = 0.0
+        self.y = 0.0
+        self.yaw = 0.0
+        self.P = np.eye(3)
+        # landmark x-y positions
+        self.lm = np.matrix(np.zeros((N_LM, LM_SIZE)))
+        # landmark position covariance
+        self.lmP = np.matrix(np.zeros((N_LM * LM_SIZE, LM_SIZE)))
+
+
+def fast_slam2(particles, u, z):
+
+    particles = predict_particles(particles, u)
+
+    particles = update_with_observation(particles, z)
+
+    particles = resampling(particles)
+
+    return particles
+
+
+def normalize_weight(particles):
+
+    sumw = sum([p.w for p in particles])
+
+    try:
+        for i in range(N_PARTICLE):
+            particles[i].w /= sumw
+    except ZeroDivisionError:
+        for i in range(N_PARTICLE):
+            particles[i].w = 1.0 / N_PARTICLE
+
+        return particles
+
+    return particles
+
+
+def calc_final_state(particles):
+
+    xEst = np.zeros((STATE_SIZE, 1))
+
+    particles = normalize_weight(particles)
+
+    for i in range(N_PARTICLE):
+        xEst[0, 0] += particles[i].w * particles[i].x
+        xEst[1, 0] += particles[i].w * particles[i].y
+        xEst[2, 0] += particles[i].w * particles[i].yaw
+
+    xEst[2, 0] = pi_2_pi(xEst[2, 0])
+    #  print(xEst)
+
+    return xEst
+
+
+def predict_particles(particles, u):
+
+    for i in range(N_PARTICLE):
+        px = np.zeros((STATE_SIZE, 1))
+        px[0, 0] = particles[i].x
+        px[1, 0] = particles[i].y
+        px[2, 0] = particles[i].yaw
+        ud = u + (np.matrix(np.random.randn(1, 2)) * R).T  # add noise
+        px = motion_model(px, ud)
+        particles[i].x = px[0, 0]
+        particles[i].y = px[1, 0]
+        particles[i].yaw = px[2, 0]
+
+    return particles
+
+
+def add_new_lm(particle, z, Q):
+
+    r = z[0, 0]
+    b = z[0, 1]
+    lm_id = int(z[0, 2])
+
+    s = math.sin(pi_2_pi(particle.yaw + b))
+    c = math.cos(pi_2_pi(particle.yaw + b))
+
+    particle.lm[lm_id, 0] = particle.x + r * c
+    particle.lm[lm_id, 1] = particle.y + r * s
+
+    # covariance
+    Gz = np.matrix([[c, -r * s],
+                    [s, r * c]])
+
+    particle.lmP[2 * lm_id:2 * lm_id + 2] = Gz * Q * Gz.T
+
+    return particle
+
+
+def compute_jacobians(particle, xf, Pf, Q):
+    dx = xf[0, 0] - particle.x
+    dy = xf[1, 0] - particle.y
+    d2 = dx**2 + dy**2
+    d = math.sqrt(d2)
+
+    zp = np.matrix([[d, pi_2_pi(math.atan2(dy, dx) - particle.yaw)]]).T
+
+    Hv = np.matrix([[-dx / d, -dy / d, 0.0],
+                    [dy / d2, -dx / d2, -1.0]])
+
+    Hf = np.matrix([[dx / d, dy / d],
+                    [-dy / d2, dx / d2]])
+
+    Sf = Hf * Pf * Hf.T + Q
+
+    return zp, Hv, Hf, Sf
+
+
+def update_KF_with_cholesky(xf, Pf, v, Q, Hf):
+    PHt = Pf * Hf.T
+    S = Hf * PHt + Q
+
+    S = (S + S.T) * 0.5
+    SChol = np.linalg.cholesky(S).T
+    SCholInv = np.linalg.inv(SChol)
+    W1 = PHt * SCholInv
+    W = W1 * SCholInv.T
+
+    x = xf + W * v
+    P = Pf - W1 * W1.T
+
+    return x, P
+
+
+def update_landmark(particle, z, Q):
+
+    lm_id = int(z[0, 2])
+    xf = np.matrix(particle.lm[lm_id, :]).T
+    Pf = np.matrix(particle.lmP[2 * lm_id:2 * lm_id + 2, :])
+
+    zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q)
+
+    dz = z[0, 0: 2].T - zp
+    dz[1, 0] = pi_2_pi(dz[1, 0])
+
+    xf, Pf = update_KF_with_cholesky(xf, Pf, dz, Q, Hf)
+
+    particle.lm[lm_id, :] = xf.T
+    particle.lmP[2 * lm_id:2 * lm_id + 2, :] = Pf
+
+    return particle
+
+
+def compute_weight(particle, z, Q):
+
+    lm_id = int(z[0, 2])
+    xf = np.matrix(particle.lm[lm_id, :]).T
+    Pf = np.matrix(particle.lmP[2 * lm_id:2 * lm_id + 2])
+    zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q)
+
+    dz = z[0, 0: 2].T - zp
+    dz[1, 0] = pi_2_pi(dz[1, 0])
+
+    S = particle.lmP[2 * lm_id:2 * lm_id + 2]
+    try:
+        invS = np.linalg.inv(S)
+    except np.linalg.linalg.LinAlgError:
+        print("singuler")
+        return 1.0
+
+    num = math.exp(-0.5 * dz.T * invS * dz)
+    den = 2.0 * math.pi * math.sqrt(np.linalg.det(S))
+
+    w = num / den
+
+    return w
+
+
+def proposal_sampling(particle, z, Q):
+
+    lm_id = int(z[0, 2])
+    xf = np.matrix(particle.lm[lm_id, :]).T
+    Pf = np.matrix(particle.lmP[2 * lm_id:2 * lm_id + 2])
+    # State
+    x = np.matrix([[particle.x, particle.y, particle.yaw]]).T
+    P = particle.P
+    zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q)
+
+    Sfi = np.linalg.inv(Sf)
+    dz = z[0, 0: 2].T - zp
+    dz[1, 0] = pi_2_pi(dz[1, 0])
+
+    Pi = np.linalg.inv(P)
+
+    particle.P = np.linalg.inv(Hv.T * Sfi * Hv + Pi)  # proposal covariance
+    x += particle.P * Hv.T * Sfi * dz  # proposal mean
+
+    particle.x = x[0, 0]
+    particle.y = x[1, 0]
+    particle.yaw = x[2, 0]
+
+    return particle
+
+
+def update_with_observation(particles, z):
+
+    for iz in range(len(z[:, 0])):
+
+        lmid = int(z[iz, 2])
+
+        for ip in range(N_PARTICLE):
+            # new landmark
+            if abs(particles[ip].lm[lmid, 0]) <= 0.01:
+                particles[ip] = add_new_lm(particles[ip], z[iz, :], Q)
+            # known landmark
+            else:
+                w = compute_weight(particles[ip], z[iz, :], Q)
+                particles[ip].w *= w
+
+                particles[ip] = update_landmark(particles[ip], z[iz, :], Q)
+                particles[ip] = proposal_sampling(particles[ip], z[iz, :], Q)
+
+    return particles
+
+
+def resampling(particles):
+    """
+    low variance re-sampling
+    """
+
+    particles = normalize_weight(particles)
+
+    pw = []
+    for i in range(N_PARTICLE):
+        pw.append(particles[i].w)
+
+    pw = np.matrix(pw)
+
+    Neff = 1.0 / (pw * pw.T)[0, 0]  # Effective particle number
+    #  print(Neff)
+
+    if Neff < NTH:  # resampling
+        wcum = np.cumsum(pw)
+        base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
+        resampleid = base + np.random.rand(base.shape[1]) / N_PARTICLE
+
+        inds = []
+        ind = 0
+        for ip in range(N_PARTICLE):
+            while ((ind < wcum.shape[1] - 1) and (resampleid[0, ip] > wcum[0, ind])):
+                ind += 1
+            inds.append(ind)
+
+        tparticles = particles[:]
+        for i in range(len(inds)):
+            particles[i].x = tparticles[inds[i]].x
+            particles[i].y = tparticles[inds[i]].y
+            particles[i].yaw = tparticles[inds[i]].yaw
+            particles[i].lm = tparticles[inds[i]].lm[:, :]
+            particles[i].lmP = tparticles[inds[i]].lmP[:, :]
+            particles[i].w = 1.0 / N_PARTICLE
+
+    return particles
+
+
+def calc_input(time):
+
+    if time <= 3.0:
+        v = 0.0
+        yawrate = 0.0
+    else:
+        v = 1.0  # [m/s]
+        yawrate = 0.1  # [rad/s]
+
+    u = np.matrix([v, yawrate]).T
+
+    return u
+
+
+def observation(xTrue, xd, u, RFID):
+
+    xTrue = motion_model(xTrue, u)
+
+    # add noise to gps x-y
+    z = np.matrix(np.zeros((0, 3)))
+
+    for i in range(len(RFID[:, 0])):
+
+        dx = RFID[i, 0] - xTrue[0, 0]
+        dy = RFID[i, 1] - xTrue[1, 0]
+        d = math.sqrt(dx**2 + dy**2)
+        angle = math.atan2(dy, dx) - xTrue[2, 0]
+        if d <= MAX_RANGE:
+            dn = d + np.random.randn() * Qsim[0, 0]  # add noise
+            anglen = angle + np.random.randn() * Qsim[1, 1]  # add noise
+            zi = np.matrix([dn, pi_2_pi(anglen), i])
+            z = np.vstack((z, zi))
+
+    # add noise to input
+    ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
+    ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1] + OFFSET_YAWRATE_NOISE
+    ud = np.matrix([ud1, ud2]).T
+
+    xd = motion_model(xd, ud)
+
+    return xTrue, z, xd, ud
+
+
+def motion_model(x, u):
+
+    F = np.matrix([[1.0, 0, 0],
+                   [0, 1.0, 0],
+                   [0, 0, 1.0]])
+
+    B = np.matrix([[DT * math.cos(x[2, 0]), 0],
+                   [DT * math.sin(x[2, 0]), 0],
+                   [0.0, DT]])
+
+    x = F * x + B * u
+
+    x[2, 0] = pi_2_pi(x[2, 0])
+
+    return x
+
+
+def pi_2_pi(angle):
+    return (angle + math.pi) % (2*math.pi) - math.pi
+
+
+def main():
+    print(__file__ + " start!!")
+
+    time = 0.0
+
+    # RFID positions [x, y]
+    RFID = np.array([[10.0, -2.0],
+                     [15.0, 10.0],
+                     [15.0, 15.0],
+                     [10.0, 20.0],
+                     [3.0, 15.0],
+                     [-5.0, 20.0],
+                     [-5.0, 5.0],
+                     [-10.0, 15.0]
+                     ])
+    N_LM = RFID.shape[0]
+
+    # State Vector [x y yaw v]'
+    xEst = np.matrix(np.zeros((STATE_SIZE, 1)))
+    xTrue = np.matrix(np.zeros((STATE_SIZE, 1)))
+
+    xDR = np.matrix(np.zeros((STATE_SIZE, 1)))  # Dead reckoning
+
+    # history
+    hxEst = xEst
+    hxTrue = xTrue
+    hxDR = xTrue
+
+    particles = [Particle(N_LM) for i in range(N_PARTICLE)]
+
+    while SIM_TIME >= time:
+        time += DT
+        u = calc_input(time)
+
+        xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
+
+        particles = fast_slam2(particles, ud, z)
+
+        xEst = calc_final_state(particles)
+
+        x_state = xEst[0: STATE_SIZE]
+
+        # store data history
+        hxEst = np.hstack((hxEst, x_state))
+        hxDR = np.hstack((hxDR, xDR))
+        hxTrue = np.hstack((hxTrue, xTrue))
+
+        if show_animation:
+            plt.cla()
+            plt.plot(RFID[:, 0], RFID[:, 1], "*k")
+
+            for iz in range(len(z[:, 0])):
+                lmid = int(z[iz, 2])
+                plt.plot([xEst[0, 0], RFID[lmid, 0]], [
+                         xEst[1, 0], RFID[lmid, 1]], "-k")
+
+            for i in range(N_PARTICLE):
+                plt.plot(particles[i].x, particles[i].y, ".r")
+                plt.plot(particles[i].lm[:, 0], particles[i].lm[:, 1], "xb")
+
+            plt.plot(np.array(hxTrue[0, :]).flatten(),
+                     np.array(hxTrue[1, :]).flatten(), "-b")
+            plt.plot(np.array(hxDR[0, :]).flatten(),
+                     np.array(hxDR[1, :]).flatten(), "-k")
+            plt.plot(np.array(hxEst[0, :]).flatten(),
+                     np.array(hxEst[1, :]).flatten(), "-r")
+
+            plt.plot(xEst[0], xEst[1], "xk")
+            plt.axis("equal")
+            plt.grid(True)
+            plt.pause(0.001)
+
+
+if __name__ == '__main__':
+    main()

SLAM/GraphBasedSLAM/graph_based_slam.py

@@ -0,0 +1,315 @@
+"""
+
+Graph based SLAM example
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+Ref
+
+[A Tutorial on Graph-Based SLAM](http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf)
+
+"""
+
+import numpy as np
+import math
+import copy
+import itertools
+import matplotlib.pyplot as plt
+
+
+#  Simulation parameter
+Qsim = np.diag([0.2, math.radians(1.0)])**2
+Rsim = np.diag([0.1, math.radians(10.0)])**2
+
+DT = 2.0  # time tick [s]
+SIM_TIME = 100.0  # simulation time [s]
+MAX_RANGE = 30.0  # maximum observation range
+STATE_SIZE = 3  # State size [x,y,yaw]
+
+# Covariance parameter of Graph Based SLAM
+C_SIGMA1 = 0.1
+C_SIGMA2 = 0.1
+C_SIGMA3 = math.radians(1.0)
+
+MAX_ITR = 20  # Maximum iteration
+
+show_graph_dtime = 20.0  # [s]
+show_animation = True
+
+
+class Edge():
+
+    def __init__(self):
+        self.e = np.zeros((3, 1))
+        self.omega = np.zeros((3, 3))  # information matrix
+        self.d1 = 0.0
+        self.d2 = 0.0
+        self.yaw1 = 0.0
+        self.yaw2 = 0.0
+        self.angle1 = 0.0
+        self.angle2 = 0.0
+        self.id1 = 0
+        self.id2 = 0
+
+
+def cal_observation_sigma(d):
+
+    sigma = np.zeros((3, 3))
+    sigma[0, 0] = C_SIGMA1**2
+    sigma[1, 1] = C_SIGMA2**2
+    sigma[2, 2] = C_SIGMA3**2
+
+    return sigma
+
+
+def calc_rotational_matrix(angle):
+
+    Rt = np.matrix([[math.cos(angle), -math.sin(angle), 0],
+                    [math.sin(angle), math.cos(angle), 0],
+                    [0, 0, 1.0]])
+    return Rt
+
+
+def calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1,
+              angle1, phi1, d2, angle2, phi2, t1, t2):
+    edge = Edge()
+
+    tangle1 = pi_2_pi(yaw1 + angle1)
+    tangle2 = pi_2_pi(yaw2 + angle2)
+    tmp1 = d1 * math.cos(tangle1)
+    tmp2 = d2 * math.cos(tangle2)
+    tmp3 = d1 * math.sin(tangle1)
+    tmp4 = d2 * math.sin(tangle2)
+
+    edge.e[0, 0] = x2 - x1 - tmp1 + tmp2
+    edge.e[1, 0] = y2 - y1 - tmp3 + tmp4
+    hyaw = phi1 - phi2 + angle1 - angle2
+    edge.e[2, 0] = pi_2_pi(yaw2 - yaw1 - hyaw)
+
+    Rt1 = calc_rotational_matrix(tangle1)
+    Rt2 = calc_rotational_matrix(tangle2)
+
+    sig1 = cal_observation_sigma(d1)
+    sig2 = cal_observation_sigma(d2)
+
+    edge.omega = np.linalg.inv(Rt1 * sig1 * Rt1.T + Rt2 * sig2 * Rt2.T)
+
+    edge.d1, edge.d2 = d1, d2
+    edge.yaw1, edge.yaw2 = yaw1, yaw2
+    edge.angle1, edge.angle2 = angle1, angle2
+    edge.id1, edge.id2 = t1, t2
+
+    return edge
+
+
+def calc_edges(xlist, zlist):
+
+    edges = []
+    cost = 0.0
+    zids = list(itertools.combinations(range(len(zlist)), 2))
+
+    for (t1, t2) in zids:
+        x1, y1, yaw1 = xlist[0, t1], xlist[1, t1], xlist[2, t1]
+        x2, y2, yaw2 = xlist[0, t2], xlist[1, t2], xlist[2, t2]
+
+        if zlist[t1] is None or zlist[t2] is None:
+            continue  # No observation
+
+        for iz1 in range(len(zlist[t1][:, 0])):
+            for iz2 in range(len(zlist[t2][:, 0])):
+                if zlist[t1][iz1, 3] == zlist[t2][iz2, 3]:
+                    d1 = zlist[t1][iz1, 0]
+                    angle1, phi1 = zlist[t1][iz1, 1], zlist[t1][iz1, 2]
+                    d2 = zlist[t2][iz2, 0]
+                    angle2, phi2 = zlist[t2][iz2, 1], zlist[t2][iz2, 2]
+
+                    edge = calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1,
+                                     angle1, phi1, d2, angle2, phi2, t1, t2)
+
+                    edges.append(edge)
+                    cost += (edge.e.T * edge.omega * edge.e)[0, 0]
+
+    print("cost:", cost, ",nedge:", len(edges))
+    return edges
+
+
+def calc_jacobian(edge):
+    t1 = edge.yaw1 + edge.angle1
+    A = np.matrix([[-1.0, 0, edge.d1 * math.sin(t1)],
+                   [0, -1.0, -edge.d1 * math.cos(t1)],
+                   [0, 0, -1.0]])
+
+    t2 = edge.yaw2 + edge.angle2
+    B = np.matrix([[1.0, 0, -edge.d2 * math.sin(t2)],
+                   [0, 1.0, edge.d2 * math.cos(t2)],
+                   [0, 0, 1.0]])
+
+    return A, B
+
+
+def fill_H_and_b(H, b, edge):
+
+    A, B = calc_jacobian(edge)
+
+    id1 = edge.id1 * STATE_SIZE
+    id2 = edge.id2 * STATE_SIZE
+
+    H[id1:id1 + STATE_SIZE, id1:id1 + STATE_SIZE] += A.T * edge.omega * A
+    H[id1:id1 + STATE_SIZE, id2:id2 + STATE_SIZE] += A.T * edge.omega * B
+    H[id2:id2 + STATE_SIZE, id1:id1 + STATE_SIZE] += B.T * edge.omega * A
+    H[id2:id2 + STATE_SIZE, id2:id2 + STATE_SIZE] += B.T * edge.omega * B
+
+    b[id1:id1 + STATE_SIZE, 0] += (A.T * edge.omega * edge.e)
+    b[id2:id2 + STATE_SIZE, 0] += (B.T * edge.omega * edge.e)
+
+    return H, b
+
+
+def graph_based_slam(x_init, hz):
+    print("start graph based slam")
+
+    zlist = copy.deepcopy(hz)
+    zlist.insert(1, zlist[0])
+
+    x_opt = copy.deepcopy(x_init)
+    nt = x_opt.shape[1]
+    n = nt * STATE_SIZE
+
+    for itr in range(MAX_ITR):
+        edges = calc_edges(x_opt, zlist)
+
+        H = np.matrix(np.zeros((n, n)))
+        b = np.matrix(np.zeros((n, 1)))
+
+        for edge in edges:
+            H, b = fill_H_and_b(H, b, edge)
+
+        # to fix origin
+        H[0:STATE_SIZE, 0:STATE_SIZE] += np.identity(STATE_SIZE)
+
+        dx = - np.linalg.inv(H).dot(b)
+
+        for i in range(nt):
+            x_opt[0:3, i] += dx[i * 3:i * 3 + 3, 0]
+
+        diff = dx.T.dot(dx)
+        print("iteration: %d, diff: %f" % (itr + 1, diff))
+        if diff < 1.0e-5:
+            break
+
+    return x_opt
+
+
+def calc_input():
+    v = 1.0  # [m/s]
+    yawrate = 0.1  # [rad/s]
+    u = np.matrix([v, yawrate]).T
+    return u
+
+
+def observation(xTrue, xd, u, RFID):
+
+    xTrue = motion_model(xTrue, u)
+
+    # add noise to gps x-y
+    z = np.matrix(np.zeros((0, 4)))
+
+    for i in range(len(RFID[:, 0])):
+
+        dx = RFID[i, 0] - xTrue[0, 0]
+        dy = RFID[i, 1] - xTrue[1, 0]
+        d = math.sqrt(dx**2 + dy**2)
+        angle = pi_2_pi(math.atan2(dy, dx)) - xTrue[2, 0]
+        phi = pi_2_pi(math.atan2(dy, dx))
+        if d <= MAX_RANGE:
+            dn = d + np.random.randn() * Qsim[0, 0]  # add noise
+            anglen = angle + np.random.randn() * Qsim[1, 1]  # add noise
+            zi = np.matrix([dn, anglen, phi, i])
+            z = np.vstack((z, zi))
+
+    # add noise to input
+    ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
+    ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
+    ud = np.matrix([ud1, ud2]).T
+
+    xd = motion_model(xd, ud)
+
+    return xTrue, z, xd, ud
+
+
+def motion_model(x, u):
+
+    F = np.matrix([[1.0, 0, 0],
+                   [0, 1.0, 0],
+                   [0, 0, 1.0]])
+
+    B = np.matrix([[DT * math.cos(x[2, 0]), 0],
+                   [DT * math.sin(x[2, 0]), 0],
+                   [0.0, DT]])
+
+    x = F * x + B * u
+
+    return x
+
+
+def pi_2_pi(angle):
+    return (angle + math.pi) % (2*math.pi) - math.pi
+
+
+def main():
+    print(__file__ + " start!!")
+
+    time = 0.0
+
+    # RFID positions [x, y, yaw]
+    RFID = np.array([[10.0, -2.0, 0.0],
+                     [15.0, 10.0, 0.0],
+                     [3.0, 15.0, 0.0],
+                     [-5.0, 20.0, 0.0],
+                     [-5.0, 5.0, 0.0]
+                     ])
+
+    # State Vector [x y yaw v]'
+    xTrue = np.matrix(np.zeros((STATE_SIZE, 1)))
+    xDR = np.matrix(np.zeros((STATE_SIZE, 1)))  # Dead reckoning
+
+    # history
+    hxTrue = xTrue
+    hxDR = xTrue
+    hz = []
+    dtime = 0.0
+
+    while SIM_TIME >= time:
+        time += DT
+        dtime += DT
+        u = calc_input()
+
+        xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
+
+        hxDR = np.hstack((hxDR, xDR))
+        hxTrue = np.hstack((hxTrue, xTrue))
+        hz.append(z)
+
+        if dtime >= show_graph_dtime:
+            x_opt = graph_based_slam(hxDR, hz)
+            dtime = 0.0
+
+            if show_animation:
+                plt.cla()
+
+                plt.plot(RFID[:, 0], RFID[:, 1], "*k")
+
+                plt.plot(np.array(hxTrue[0, :]).flatten(),
+                         np.array(hxTrue[1, :]).flatten(), "-b")
+                plt.plot(np.array(hxDR[0, :]).flatten(),
+                         np.array(hxDR[1, :]).flatten(), "-k")
+                plt.plot(np.array(x_opt[0, :]).flatten(),
+                         np.array(x_opt[1, :]).flatten(), "-r")
+                plt.axis("equal")
+                plt.grid(True)
+                plt.title("Time" + str(time)[0:5])
+                plt.pause(1.0)
+
+
+if __name__ == '__main__':
+    main()

_config.yml

@@ -1 +1,2 @@
-theme: jekyll-theme-slate
+theme: jekyll-theme-slate
+show_downloads: true

docs/Makefile

@@ -0,0 +1,20 @@
+# Minimal makefile for Sphinx documentation
+#
+
+# You can set these variables from the command line.
+SPHINXOPTS    =
+SPHINXBUILD   = sphinx-build
+SPHINXPROJ    = PythonRobotics
+SOURCEDIR     = .
+BUILDDIR      = _build
+
+# Put it first so that "make" without argument is like "make help".
+help:
+	@$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
+
+.PHONY: help Makefile
+
+# Catch-all target: route all unknown targets to Sphinx using the new
+# "make mode" option.  $(O) is meant as a shortcut for $(SPHINXOPTS).
+%: Makefile
+	@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)

docs/README.md

@@ -0,0 +1,25 @@
+## Python Robotics Documentation
+
+This folder contains documentation for the Python Robotics project.
+
+
+### Build the Documentation
+
+#### Install Sphinx and Theme
+
+```
+pip install sphinx sphinx-autobuild sphinx-rtd-theme
+```
+
+#### Building the Docs
+
+In the `docs/` folder:
+```
+make html
+```
+
+if you want to building each time a file is changed:
+
+```
+sphinx-autobuild . _build/html
+```

docs/conf.py

@@ -0,0 +1,169 @@
+# -*- coding: utf-8 -*-
+#
+# Configuration file for the Sphinx documentation builder.
+#
+# This file does only contain a selection of the most common options. For a
+# full list see the documentation:
+# http://www.sphinx-doc.org/en/master/config
+
+# -- Path setup --------------------------------------------------------------
+
+# If extensions (or modules to document with autodoc) are in another directory,
+# add these directories to sys.path here. If the directory is relative to the
+# documentation root, use os.path.abspath to make it absolute, like shown here.
+#
+import os
+# import sys
+# sys.path.insert(0, os.path.abspath('.'))
+
+
+# -- Project information -----------------------------------------------------
+
+project = 'PythonRobotics'
+copyright = '2018, Atsushi Sakai'
+author = 'Atsushi Sakai'
+
+# The short X.Y version
+version = ''
+# The full version, including alpha/beta/rc tags
+release = ''
+
+
+# -- General configuration ---------------------------------------------------
+
+# If your documentation needs a minimal Sphinx version, state it here.
+#
+# needs_sphinx = '1.0'
+
+# Add any Sphinx extension module names here, as strings. They can be
+# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
+# ones.
+extensions = [
+    'sphinx.ext.autodoc',
+    'sphinx.ext.mathjax',
+    'sphinx.ext.viewcode',
+]
+
+# Add any paths that contain templates here, relative to this directory.
+templates_path = ['_templates']
+
+# The suffix(es) of source filenames.
+# You can specify multiple suffix as a list of string:
+#
+# source_suffix = ['.rst', '.md']
+source_suffix = '.rst'
+
+# The master toctree document.
+master_doc = 'index'
+
+# The language for content autogenerated by Sphinx. Refer to documentation
+# for a list of supported languages.
+#
+# This is also used if you do content translation via gettext catalogs.
+# Usually you set "language" from the command line for these cases.
+language = None
+
+# List of patterns, relative to source directory, that match files and
+# directories to ignore when looking for source files.
+# This pattern also affects html_static_path and html_extra_path .
+exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store']
+
+# The name of the Pygments (syntax highlighting) style to use.
+pygments_style = 'sphinx'
+
+
+# -- Options for HTML output -------------------------------------------------
+
+# The theme to use for HTML and HTML Help pages.  See the documentation for
+# a list of builtin themes.
+#
+# Fix for read the docs
+on_rtd = os.environ.get('READTHEDOCS') == 'True'
+if on_rtd:
+    html_theme = 'default'
+else:
+    html_theme = 'sphinx_rtd_theme'
+
+# Theme options are theme-specific and customize the look and feel of a theme
+# further.  For a list of options available for each theme, see the
+# documentation.
+#
+html_logo = '../icon.png'
+html_theme_options = {
+    'display_version': False,
+}
+
+# Add any paths that contain custom static files (such as style sheets) here,
+# relative to this directory. They are copied after the builtin static files,
+# so a file named "default.css" will overwrite the builtin "default.css".
+html_static_path = ['_static']
+
+# Custom sidebar templates, must be a dictionary that maps document names
+# to template names.
+#
+# The default sidebars (for documents that don't match any pattern) are
+# defined by theme itself.  Builtin themes are using these templates by
+# default: ``['localtoc.html', 'relations.html', 'sourcelink.html',
+# 'searchbox.html']``.
+#
+# html_sidebars = {}
+
+
+# -- Options for HTMLHelp output ---------------------------------------------
+
+# Output file base name for HTML help builder.
+htmlhelp_basename = 'PythonRoboticsdoc'
+
+
+# -- Options for LaTeX output ------------------------------------------------
+
+latex_elements = {
+    # The paper size ('letterpaper' or 'a4paper').
+    #
+    # 'papersize': 'letterpaper',
+
+    # The font size ('10pt', '11pt' or '12pt').
+    #
+    # 'pointsize': '10pt',
+
+    # Additional stuff for the LaTeX preamble.
+    #
+    # 'preamble': '',
+
+    # Latex figure (float) alignment
+    #
+    # 'figure_align': 'htbp',
+}
+
+# Grouping the document tree into LaTeX files. List of tuples
+# (source start file, target name, title,
+#  author, documentclass [howto, manual, or own class]).
+latex_documents = [
+    (master_doc, 'PythonRobotics.tex', 'PythonRobotics Documentation',
+     'Atsushi Sakai', 'manual'),
+]
+
+
+# -- Options for manual page output ------------------------------------------
+
+# One entry per manual page. List of tuples
+# (source start file, name, description, authors, manual section).
+man_pages = [
+    (master_doc, 'pythonrobotics', 'PythonRobotics Documentation',
+     [author], 1)
+]
+
+
+# -- Options for Texinfo output ----------------------------------------------
+
+# Grouping the document tree into Texinfo files. List of tuples
+# (source start file, target name, title, author,
+#  dir menu entry, description, category)
+texinfo_documents = [
+    (master_doc, 'PythonRobotics', 'PythonRobotics Documentation',
+     author, 'PythonRobotics', 'One line description of project.',
+     'Miscellaneous'),
+]
+
+
+# -- Extension configuration -------------------------------------------------

docs/getting_started.rst

@@ -0,0 +1,29 @@
+.. _getting_started:
+
+Getting Started
+===============
+
+Requirements
+-------------
+
+-  Python 3.6.x
+-  numpy
+-  scipy
+-  matplotlib
+-  pandas
+-  `cvxpy`_
+
+.. _cvxpy: http://www.cvxpy.org/en/latest/
+
+
+How to use
+----------
+
+1. Install the required libraries. You can use environment.yml with
+   conda command.
+
+2. Clone this repo.
+
+3. Execute python script in each directory.
+
+4. Add star to this repo if you like it 😃.

docs/index.rst

@@ -0,0 +1,50 @@
+.. PythonRobotics documentation master file, created by
+   sphinx-quickstart on Sat Sep 15 13:15:55 2018.
+   You can adapt this file completely to your liking, but it should at least
+   contain the root `toctree` directive.
+
+Welcome to PythonRobotics's documentation!
+==========================================
+
+Python codes for robotics algorithm. The project is on `GitHub`_.
+
+This is a Python code collection of robotics algorithms, especially for autonomous navigation.
+
+Features:
+
+1. Easy to read for understanding each algorithm's basic idea.
+
+2. Widely used and practical algorithms are selected.
+
+3. Minimum dependency.
+
+See this paper for more details:
+
+-  `[1808.10703] PythonRobotics: a Python code collection of robotics
+   algorithms`_ (`BibTeX`_)
+
+.. _`[1808.10703] PythonRobotics: a Python code collection of robotics algorithms`: https://arxiv.org/abs/1808.10703
+.. _BibTeX: https://github.com/AtsushiSakai/PythonRoboticsPaper/blob/master/python_robotics.bib
+.. _GitHub: https://github.com/AtsushiSakai/PythonRobotics
+
+
+.. toctree::
+   :maxdepth: 2
+   :caption: Guide
+
+   getting_started
+   modules/localization
+   modules/mapping
+   modules/slam
+   modules/path_planning
+   modules/path_tracking
+   modules/arm_navigation
+   modules/aerial_navigation
+
+
+Indices and tables
+==================
+
+* :ref:`genindex`
+* :ref:`modindex`
+* :ref:`search`

docs/make.bat

@@ -0,0 +1,36 @@
+@ECHO OFF
+
+pushd %~dp0
+
+REM Command file for Sphinx documentation
+
+if "%SPHINXBUILD%" == "" (
+	set SPHINXBUILD=sphinx-build
+)
+set SOURCEDIR=.
+set BUILDDIR=_build
+set SPHINXPROJ=PythonRobotics
+
+if "%1" == "" goto help
+
+%SPHINXBUILD% >NUL 2>NUL
+if errorlevel 9009 (
+	echo.
+	echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
+	echo.installed, then set the SPHINXBUILD environment variable to point
+	echo.to the full path of the 'sphinx-build' executable. Alternatively you
+	echo.may add the Sphinx directory to PATH.
+	echo.
+	echo.If you don't have Sphinx installed, grab it from
+	echo.http://sphinx-doc.org/
+	exit /b 1
+)
+
+%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS%
+goto end
+
+:help
+%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS%
+
+:end
+popd

docs/modules/aerial_navigation.rst

@@ -0,0 +1,13 @@
+.. _aerial_navigation:
+
+Aerial Navigation
+=================
+
+Drone 3d trajectory following
+-----------------------------
+
+This is a 3d trajectory following simulation for a quadrotor.
+
+|3|
+
+.. |3| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/AerialNavigation/drone_3d_trajectory_following/animation.gif

docs/modules/arm_navigation.rst

@@ -0,0 +1,33 @@
+.. _arm_navigation:
+
+Arm Navigation
+==============
+
+Two joint arm to point control
+------------------------------
+
+Two joint arm to a point control simulation.
+
+This is a interactive simulation.
+
+You can set the goal position of the end effector with left-click on the
+ploting area.
+
+|3|
+
+N joint arm to point control
+----------------------------
+
+N joint arm to a point control simulation.
+
+This is a interactive simulation.
+
+You can set the goal position of the end effector with left-click on the
+ploting area.
+
+|4|
+
+In this simulation N = 10, however, you can change it.
+
+.. |3| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/ArmNavigation/two_joint_arm_to_point_control/animation.gif
+.. |4| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/ArmNavigation/n_joint_arm_to_point_control/animation.gif

docs/modules/localization.rst

@@ -0,0 +1,89 @@
+.. _localization:
+
+Localization
+============
+
+Extended Kalman Filter localization
+-----------------------------------
+
+.. raw:: html
+
+   <img src="https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/extended_kalman_filter/animation.gif" width="640">
+
+This is a sensor fusion localization with Extended Kalman Filter(EKF).
+
+The blue line is true trajectory, the black line is dead reckoning
+trajectory,
+
+the green point is positioning observation (ex. GPS), and the red line
+is estimated trajectory with EKF.
+
+The red ellipse is estimated covariance ellipse with EKF.
+
+Ref:
+
+-  `PROBABILISTIC ROBOTICS`_
+
+Unscented Kalman Filter localization
+------------------------------------
+
+|2|
+
+This is a sensor fusion localization with Unscented Kalman Filter(UKF).
+
+The lines and points are same meaning of the EKF simulation.
+
+Ref:
+
+-  `Discriminatively Trained Unscented Kalman Filter for Mobile Robot
+   Localization`_
+
+Particle filter localization
+----------------------------
+
+|3|
+
+This is a sensor fusion localization with Particle Filter(PF).
+
+The blue line is true trajectory, the black line is dead reckoning
+trajectory,
+
+and the red line is estimated trajectory with PF.
+
+It is assumed that the robot can measure a distance from landmarks
+(RFID).
+
+This measurements are used for PF localization.
+
+Ref:
+
+-  `PROBABILISTIC ROBOTICS`_
+
+Histogram filter localization
+-----------------------------
+
+|4|
+
+This is a 2D localization example with Histogram filter.
+
+The red cross is true position, black points are RFID positions.
+
+The blue grid shows a position probability of histogram filter.
+
+In this simulation, x,y are unknown, yaw is known.
+
+The filter integrates speed input and range observations from RFID for
+localization.
+
+Initial position is not needed.
+
+Ref:
+
+-  `PROBABILISTIC ROBOTICS`_
+
+.. _PROBABILISTIC ROBOTICS: http://www.probabilistic-robotics.org/
+.. _Discriminatively Trained Unscented Kalman Filter for Mobile Robot Localization: https://www.researchgate.net/publication/267963417_Discriminatively_Trained_Unscented_Kalman_Filter_for_Mobile_Robot_Localization
+
+.. |2| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/unscented_kalman_filter/animation.gif
+.. |3| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/particle_filter/animation.gif
+.. |4| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/histogram_filter/animation.gif

docs/modules/mapping.rst

@@ -0,0 +1,43 @@
+.. _mapping:
+
+Mapping
+=======
+
+Gaussian grid map
+-----------------
+
+This is a 2D Gaussian grid mapping example.
+
+|2|
+
+Ray casting grid map
+--------------------
+
+This is a 2D ray casting grid mapping example.
+
+|3|
+
+k-means object clustering
+-------------------------
+
+This is a 2D object clustering with k-means algorithm.
+
+|4|
+
+Object shape recognition using circle fitting
+---------------------------------------------
+
+This is an object shape recognition using circle fitting.
+
+|5|
+
+The blue circle is the true object shape.
+
+The red crosses are observations from a ranging sensor.
+
+The red circle is the estimated object shape using circle fitting.
+
+.. |2| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/Mapping/gaussian_grid_map/animation.gif
+.. |3| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/Mapping/raycasting_grid_map/animation.gif
+.. |4| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/Mapping/kmeans_clustering/animation.gif
+.. |5| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/Mapping/circle_fitting/animation.gif

docs/modules/path_planning.rst

@@ -0,0 +1,456 @@
+.. _path_planning:
+
+Path Planning
+=============
+
+Dynamic Window Approach
+-----------------------
+
+This is a 2D navigation sample code with Dynamic Window Approach.
+
+-  `The Dynamic Window Approach to Collision
+   Avoidance <https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf>`__
+
+|DWA|
+
+Grid based search
+-----------------
+
+Dijkstra algorithm
+~~~~~~~~~~~~~~~~~~
+
+This is a 2D grid based shortest path planning with Dijkstra's
+algorithm.
+
+|Dijkstra|
+
+In the animation, cyan points are searched nodes.
+
+.. _a*-algorithm:
+
+A\* algorithm
+~~~~~~~~~~~~~
+
+This is a 2D grid based shortest path planning with A star algorithm.
+
+|astar|
+
+In the animation, cyan points are searched nodes.
+
+Its heuristic is 2D Euclid distance.
+
+Potential Field algorithm
+~~~~~~~~~~~~~~~~~~~~~~~~~
+
+This is a 2D grid based path planning with Potential Field algorithm.
+
+|PotentialField|
+
+In the animation, the blue heat map shows potential value on each grid.
+
+Ref:
+
+-  `Robotic Motion Planning:Potential
+   Functions <https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf>`__
+
+Model Predictive Trajectory Generator
+-------------------------------------
+
+This is a path optimization sample on model predictive trajectory
+generator.
+
+This algorithm is used for state lattice planner.
+
+Path optimization sample
+~~~~~~~~~~~~~~~~~~~~~~~~
+
+|4|
+
+Lookup table generation sample
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+|5|
+
+Ref:
+
+-  `Optimal rough terrain trajectory generation for wheeled mobile
+   robots <http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328>`__
+
+State Lattice Planning
+----------------------
+
+This script is a path planning code with state lattice planning.
+
+This code uses the model predictive trajectory generator to solve
+boundary problem.
+
+Ref:
+
+-  `Optimal rough terrain trajectory generation for wheeled mobile
+   robots <http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328>`__
+
+-  `State Space Sampling of Feasible Motions for High-Performance Mobile
+   Robot Navigation in Complex
+   Environments <http://www.frc.ri.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf>`__
+
+Uniform polar sampling
+~~~~~~~~~~~~~~~~~~~~~~
+
+|6|
+
+Biased polar sampling
+~~~~~~~~~~~~~~~~~~~~~
+
+|7|
+
+Lane sampling
+~~~~~~~~~~~~~
+
+|8|
+
+.. _probabilistic-road-map-(prm)-planning:
+
+Probabilistic Road-Map (PRM) planning
+-------------------------------------
+
+|PRM|
+
+This PRM planner uses Dijkstra method for graph search.
+
+In the animation, blue points are sampled points,
+
+Cyan crosses means searched points with Dijkstra method,
+
+The red line is the final path of PRM.
+
+Ref:
+
+-  `Probabilistic roadmap -
+   Wikipedia <https://en.wikipedia.org/wiki/Probabilistic_roadmap>`__
+
+　　
+
+Voronoi Road-Map planning
+-------------------------
+
+|VRM|
+
+This Voronoi road-map planner uses Dijkstra method for graph search.
+
+In the animation, blue points are Voronoi points,
+
+Cyan crosses mean searched points with Dijkstra method,
+
+The red line is the final path of Vornoi Road-Map.
+
+Ref:
+
+-  `Robotic Motion
+   Planning <https://www.cs.cmu.edu/~motionplanning/lecture/Chap5-RoadMap-Methods_howie.pdf>`__
+
+.. _rapidly-exploring-random-trees-(rrt):
+
+Rapidly-Exploring Random Trees (RRT)
+------------------------------------
+
+Basic RRT
+~~~~~~~~~
+
+|9|
+
+This is a simple path planning code with Rapidly-Exploring Random Trees
+(RRT)
+
+Black circles are obstacles, green line is a searched tree, red crosses
+are start and goal positions.
+
+.. _rrt*:
+
+RRT\*
+~~~~~
+
+|10|
+
+This is a path planning code with RRT\*
+
+Black circles are obstacles, green line is a searched tree, red crosses
+are start and goal positions.
+
+Ref:
+
+-  `Incremental Sampling-based Algorithms for Optimal Motion
+   Planning <https://arxiv.org/abs/1005.0416>`__
+
+-  `Sampling-based Algorithms for Optimal Motion
+   Planning <http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf>`__
+
+RRT with dubins path
+~~~~~~~~~~~~~~~~~~~~
+
+|PythonRobotics|
+
+Path planning for a car robot with RRT and dubins path planner.
+
+.. _rrt*-with-dubins-path:
+
+RRT\* with dubins path
+~~~~~~~~~~~~~~~~~~~~~~
+
+|AtsushiSakai/PythonRobotics|
+
+Path planning for a car robot with RRT\* and dubins path planner.
+
+.. _rrt*-with-reeds-sheep-path:
+
+RRT\* with reeds-sheep path
+~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+|11|
+
+Path planning for a car robot with RRT\* and reeds sheep path planner.
+
+.. _informed-rrt*:
+
+Informed RRT\*
+~~~~~~~~~~~~~~
+
+|irrt|
+
+This is a path planning code with Informed RRT*.
+
+The cyan ellipse is the heuristic sampling domain of Informed RRT*.
+
+Ref:
+
+-  `Informed RRT\*: Optimal Sampling-based Path Planning Focused via
+   Direct Sampling of an Admissible Ellipsoidal
+   Heuristic <https://arxiv.org/pdf/1404.2334.pdf>`__
+
+.. _batch-informed-rrt*:
+
+Batch Informed RRT\*
+~~~~~~~~~~~~~~~~~~~~
+
+|irrt2|
+
+This is a path planning code with Batch Informed RRT*.
+
+Ref:
+
+-  `Batch Informed Trees (BIT*): Sampling-based Optimal Planning via the
+   Heuristically Guided Search of Implicit Random Geometric
+   Graphs <https://arxiv.org/abs/1405.5848>`__
+
+.. _closed-loop-rrt*:
+
+Closed Loop RRT\*
+~~~~~~~~~~~~~~~~~
+
+A vehicle model based path planning with closed loop RRT*.
+
+|CLRRT|
+
+In this code, pure-pursuit algorithm is used for steering control,
+
+PID is used for speed control.
+
+Ref:
+
+-  `Motion Planning in Complex Environments using Closed-loop
+   Prediction <http://acl.mit.edu/papers/KuwataGNC08.pdf>`__
+
+-  `Real-time Motion Planning with Applications to Autonomous Urban
+   Driving <http://acl.mit.edu/papers/KuwataTCST09.pdf>`__
+
+-  `[1601.06326] Sampling-based Algorithms for Optimal Motion Planning
+   Using Closed-loop Prediction <https://arxiv.org/abs/1601.06326>`__
+
+.. _lqr-rrt*:
+
+LQR-RRT\*
+~~~~~~~~~
+
+This is a path planning simulation with LQR-RRT*.
+
+A double integrator motion model is used for LQR local planner.
+
+|LQRRRT|
+
+Ref:
+
+-  `LQR-RRT\*: Optimal Sampling-Based Motion Planning with Automatically
+   Derived Extension
+   Heuristics <http://lis.csail.mit.edu/pubs/perez-icra12.pdf>`__
+
+-  `MahanFathi/LQR-RRTstar: LQR-RRT\* method is used for random motion
+   planning of a simple pendulum in its phase
+   plot <https://github.com/MahanFathi/LQR-RRTstar>`__
+
+Cubic spline planning
+---------------------
+
+A sample code for cubic path planning.
+
+This code generates a curvature continuous path based on x-y waypoints
+with cubic spline.
+
+Heading angle of each point can be also calculated analytically.
+
+|12|
+|13|
+|14|
+
+B-Spline planning
+-----------------
+
+|B-Spline|
+
+This is a path planning with B-Spline curse.
+
+If you input waypoints, it generates a smooth path with B-Spline curve.
+
+The final course should be on the first and last waypoints.
+
+Ref:
+
+-  `B-spline - Wikipedia <https://en.wikipedia.org/wiki/B-spline>`__
+
+.. _eta^3-spline-path-planning:
+
+Eta^3 Spline path planning
+--------------------------
+
+|eta3|
+
+This is a path planning with Eta^3 spline.
+
+Ref:
+
+-  `\\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile
+   Robots <https://ieeexplore.ieee.org/document/4339545/>`__
+
+Bezier path planning
+--------------------
+
+A sample code of Bezier path planning.
+
+It is based on 4 control points Beier path.
+
+|Bezier1|
+
+If you change the offset distance from start and end point,
+
+You can get different Beizer course:
+
+|Bezier2|
+
+Ref:
+
+-  `Continuous Curvature Path Generation Based on Bezier Curves for
+   Autonomous
+   Vehicles <http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.294.6438&rep=rep1&type=pdf>`__
+
+Quintic polynomials planning
+----------------------------
+
+Motion planning with quintic polynomials.
+
+|2|
+
+It can calculate 2D path, velocity, and acceleration profile based on
+quintic polynomials.
+
+Ref:
+
+-  `Local Path Planning And Motion Control For Agv In
+   Positioning <http://ieeexplore.ieee.org/document/637936/>`__
+
+Dubins path planning
+--------------------
+
+A sample code for Dubins path planning.
+
+|dubins|
+
+Ref:
+
+-  `Dubins path -
+   Wikipedia <https://en.wikipedia.org/wiki/Dubins_path>`__
+
+Reeds Shepp planning
+--------------------
+
+A sample code with Reeds Shepp path planning.
+
+|RSPlanning|
+
+Ref:
+
+-  `15.3.2 Reeds-Shepp
+   Curves <http://planning.cs.uiuc.edu/node822.html>`__
+
+-  `optimal paths for a car that goes both forwards and
+   backwards <https://pdfs.semanticscholar.org/932e/c495b1d0018fd59dee12a0bf74434fac7af4.pdf>`__
+
+-  `ghliu/pyReedsShepp: Implementation of Reeds Shepp
+   curve. <https://github.com/ghliu/pyReedsShepp>`__
+
+LQR based path planning
+-----------------------
+
+A sample code using LQR based path planning for double integrator model.
+
+|RSPlanning2|
+
+Optimal Trajectory in a Frenet Frame
+------------------------------------
+
+|15|
+
+This is optimal trajectory generation in a Frenet Frame.
+
+The cyan line is the target course and black crosses are obstacles.
+
+The red line is predicted path.
+
+Ref:
+
+-  `Optimal Trajectory Generation for Dynamic Street Scenarios in a
+   Frenet
+   Frame <https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf>`__
+
+-  `Optimal trajectory generation for dynamic street scenarios in a
+   Frenet Frame <https://www.youtube.com/watch?v=Cj6tAQe7UCY>`__
+
+.. |DWA| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/DynamicWindowApproach/animation.gif
+.. |Dijkstra| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/Dijkstra/animation.gif
+.. |astar| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/AStar/animation.gif
+.. |PotentialField| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/PotentialFieldPlanning/animation.gif
+.. |4| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/ModelPredictiveTrajectoryGenerator/kn05animation.gif
+.. |5| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png?raw=True
+.. |6| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/StateLatticePlanner/UniformPolarSampling.gif
+.. |7| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/StateLatticePlanner/BiasedPolarSampling.gif
+.. |8| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/StateLatticePlanner/LaneSampling.gif
+.. |PRM| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/ProbabilisticRoadMap/animation.gif
+.. |VRM| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/VoronoiRoadMap/animation.gif
+.. |9| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/RRT/animation.gif
+.. |10| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/RRTstar/animation.gif
+.. |PythonRobotics| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/RRTDubins/animation.gif
+.. |AtsushiSakai/PythonRobotics| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/RRTStarDubins/animation.gif
+.. |11| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/RRTStarReedsShepp/animation.gif
+.. |irrt| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/InformedRRTStar/animation.gif
+.. |irrt2| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/BatchInformedRRTStar/animation.gif
+.. |CLRRT| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/ClosedLoopRRTStar/animation.gif
+.. |LQRRRT| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/LQRRRTStar/animation.gif
+.. |12| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/CubicSpline/Figure_1.png?raw=True
+.. |13| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/CubicSpline/Figure_2.png?raw=True
+.. |14| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/CubicSpline/Figure_3.png?raw=True
+.. |B-Spline| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/BSplinePath/Figure_1.png?raw=True
+.. |eta3| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/Eta3SplinePath/animation.gif?raw=True
+.. |Bezier1| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/BezierPath/Figure_1.png?raw=True
+.. |Bezier2| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/BezierPath/Figure_2.png?raw=True
+.. |2| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/QuinticPolynomialsPlanner/animation.gif
+.. |dubins| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/DubinsPath/animation.gif?raw=True
+.. |RSPlanning| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/ReedsSheppPath/animation.gif?raw=true
+.. |RSPlanning2| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/LQRPlanner/animation.gif?raw=true
+.. |15| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/FrenetOptimalTrajectory/animation.gif

docs/modules/path_tracking.rst

@@ -0,0 +1,112 @@
+.. _path_tracking:
+
+Path tracking
+=============
+
+move to a pose control
+----------------------
+
+This is a simulation of moving to a pose control
+
+|2|
+
+Ref:
+
+-  `P. I. Corke, "Robotics, Vision and Control" \| SpringerLink
+   p102 <https://link.springer.com/book/10.1007/978-3-642-20144-8>`__
+
+Pure pursuit tracking
+---------------------
+
+Path tracking simulation with pure pursuit steering control and PID
+speed control.
+
+|3|
+
+The red line is a target course, the green cross means the target point
+for pure pursuit control, the blue line is the tracking.
+
+Ref:
+
+-  `A Survey of Motion Planning and Control Techniques for Self-driving
+   Urban Vehicles <https://arxiv.org/abs/1604.07446>`__
+
+Stanley control
+---------------
+
+Path tracking simulation with Stanley steering control and PID speed
+control.
+
+|4|
+
+Ref:
+
+-  `Stanley: The robot that won the DARPA grand
+   challenge <http://robots.stanford.edu/papers/thrun.stanley05.pdf>`__
+
+-  `Automatic Steering Methods for Autonomous Automobile Path
+   Tracking <https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf>`__
+
+Rear wheel feedback control
+---------------------------
+
+Path tracking simulation with rear wheel feedback steering control and
+PID speed control.
+
+|5|
+
+Ref:
+
+-  `A Survey of Motion Planning and Control Techniques for Self-driving
+   Urban Vehicles <https://arxiv.org/abs/1604.07446>`__
+
+.. _linearquadratic-regulator-(lqr)-steering-control:
+
+Linear–quadratic regulator (LQR) steering control
+-------------------------------------------------
+
+Path tracking simulation with LQR steering control and PID speed
+control.
+
+|6|
+
+Ref:
+
+-  `ApolloAuto/apollo: An open autonomous driving
+   platform <https://github.com/ApolloAuto/apollo>`__
+
+.. _linearquadratic-regulator-(lqr)-speed-and-steering-control:
+
+Linear–quadratic regulator (LQR) speed and steering control
+-----------------------------------------------------------
+
+Path tracking simulation with LQR speed and steering control.
+
+|7|
+
+Ref:
+
+-  `Towards fully autonomous driving: Systems and algorithms - IEEE
+   Conference
+   Publication <http://ieeexplore.ieee.org/document/5940562/>`__
+
+Model predictive speed and steering control
+-------------------------------------------
+
+Path tracking simulation with iterative linear model predictive speed
+and steering control.
+
+.. raw:: html
+
+   <img src="https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/model_predictive_speed_and_steer_control/animation.gif" width="640">
+
+Ref:
+
+-  `notebook <https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/model_predictive_speed_and_steer_control/notebook.ipynb>`__
+
+.. |2| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/move_to_pose/animation.gif
+.. |3| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/pure_pursuit/animation.gif
+.. |4| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/stanley_controller/animation.gif
+.. |5| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/rear_wheel_feedback/animation.gif
+.. |6| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/lqr_steer_control/animation.gif
+.. |7| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathTracking/lqr_speed_steer_control/animation.gif

docs/modules/slam.rst

@@ -0,0 +1,104 @@
+.. _slam:
+
+SLAM
+====
+
+Simultaneous Localization and Mapping(SLAM) examples
+
+.. _iterative-closest-point-(icp)-matching:
+
+Iterative Closest Point (ICP) Matching
+--------------------------------------
+
+This is a 2D ICP matching example with singular value decomposition.
+
+It can calculate a rotation matrix and a translation vector between
+points to points.
+
+|3|
+
+Ref:
+
+-  `Introduction to Mobile Robotics: Iterative Closest Point Algorithm`_
+
+EKF SLAM
+--------
+
+This is an Extended Kalman Filter based SLAM example.
+
+The blue line is ground truth, the black line is dead reckoning, the red
+line is the estimated trajectory with EKF SLAM.
+
+The green crosses are estimated landmarks.
+
+|4|
+
+Ref:
+
+-  `PROBABILISTIC ROBOTICS`_
+
+FastSLAM 1.0
+------------
+
+This is a feature based SLAM example using FastSLAM 1.0.
+
+The blue line is ground truth, the black line is dead reckoning, the red
+line is the estimated trajectory with FastSLAM.
+
+The red points are particles of FastSLAM.
+
+Black points are landmarks, blue crosses are estimated landmark
+positions by FastSLAM.
+
+|5|
+
+Ref:
+
+-  `PROBABILISTIC ROBOTICS`_
+
+-  `SLAM simulations by Tim Bailey`_
+
+FastSLAM 2.0
+------------
+
+This is a feature based SLAM example using FastSLAM 2.0.
+
+The animation has the same meanings as one of FastSLAM 1.0.
+
+|6|
+
+Ref:
+
+-  `PROBABILISTIC ROBOTICS`_
+
+-  `SLAM simulations by Tim Bailey`_
+
+Graph based SLAM
+----------------
+
+This is a graph based SLAM example.
+
+The blue line is ground truth.
+
+The black line is dead reckoning.
+
+The red line is the estimated trajectory with Graph based SLAM.
+
+The black stars are landmarks for graph edge generation.
+
+|7|
+
+Ref:
+
+-  `A Tutorial on Graph-Based SLAM`_
+
+.. _`Introduction to Mobile Robotics: Iterative Closest Point Algorithm`: https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf
+.. _PROBABILISTIC ROBOTICS: http://www.probabilistic-robotics.org/
+.. _SLAM simulations by Tim Bailey: http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm
+.. _A Tutorial on Graph-Based SLAM: http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf
+
+.. |3| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/SLAM/iterative_closest_point/animation.gif
+.. |4| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/SLAM/EKFSLAM/animation.gif
+.. |5| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/SLAM/FastSLAM1/animation.gif
+.. |6| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/SLAM/FastSLAM2/animation.gif
+.. |7| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/SLAM/GraphBasedSLAM/animation.gif

environment.yml

@@ -0,0 +1,9 @@
+name: python_robotics
+dependencies:
+- python==3.6
+- matplotlib
+- scipy
+- numpy
+- pandas
+- pip:
+    - cvxpy

requirements.txt

@@ -0,0 +1,5 @@
+numpy
+pandas
+scipy
+matplotlib
+cvxpy

tests/test_LQR_planner.py

@@ -0,0 +1,15 @@
+from unittest import TestCase
+
+import sys
+sys.path.append("./PathPlanning/LQRPlanner")
+
+from PathPlanning.LQRPlanner import LQRplanner as m
+
+print(__file__)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()

tests/test_batch_informed_rrt_star.py

@@ -0,0 +1,12 @@
+from unittest import TestCase
+
+from PathPlanning.BatchInformedRRTStar import batch_informed_rrtstar as m
+
+print(__file__)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()

tests/test_circle_fitting.py

@@ -0,0 +1,12 @@
+from unittest import TestCase
+
+from Mapping.circle_fitting import circle_fitting as m
+
+print(__file__)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()

tests/test_drone_3d_trajectory_following.py

@@ -0,0 +1,14 @@
+from unittest import TestCase
+
+import sys
+sys.path.append("./AerialNavigation/drone_3d_trajectory_following/")
+
+from AerialNavigation.drone_3d_trajectory_following import drone_3d_trajectory_following as m
+print(__file__)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()

tests/test_eta3_spline_path.py

@@ -0,0 +1,10 @@
+
+from unittest import TestCase
+from PathPlanning.Eta3SplinePath import eta3_spline_path as m
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()

tests/test_fast_slam1.py

@@ -0,0 +1,12 @@
+from unittest import TestCase
+
+from SLAM.FastSLAM1 import fast_slam1 as m
+
+print(__file__)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()

tests/test_fast_slam2.py

@@ -0,0 +1,12 @@
+from unittest import TestCase
+
+from SLAM.FastSLAM2 import fast_slam2 as m
+
+print(__file__)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()

tests/test_graph_based_slam.py

@@ -0,0 +1,12 @@
+from unittest import TestCase
+
+from SLAM.GraphBasedSLAM import graph_based_slam as m
+
+print(__file__)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()

tests/test_histogram_filter.py

@@ -0,0 +1,12 @@
+from unittest import TestCase
+
+from Localization.histogram_filter import histogram_filter as m
+
+print(__file__)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()

tests/test_informed_rrt_star.py

@@ -0,0 +1,12 @@
+from unittest import TestCase
+
+from PathPlanning.InformedRRTStar import informed_rrt_star as m
+
+print(__file__)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()

tests/test_kmeans_clustering.py

@@ -0,0 +1,12 @@
+from unittest import TestCase
+
+from Mapping.kmeans_clustering import kmeans_clustering as m
+
+print(__file__)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()