'update'

.idea/.gitignore

@@ -1,3 +1,3 @@
+
 # Default ignored files
-/shelf/
-/workspace.xml
+/workspace.xml

Stochastic Shortest Path/.idea/Stochastic Shortest Path.iml → .idea/PathPlanning.iml

@@ -2,10 +2,11 @@
 <module type="PYTHON_MODULE" version="4">
   <component name="NewModuleRootManager">
     <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="Python 3.7 (Search-based Planning)" jdkType="Python SDK" />
+    <orderEntry type="inheritedJdk" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
   <component name="TestRunnerService">
+    <option name="projectConfiguration" value="pytest" />
     <option name="PROJECT_TEST_RUNNER" value="pytest" />
   </component>
 </module>

.idea/modules.xml

@@ -2,7 +2,7 @@
 <project version="4">
   <component name="ProjectModuleManager">
     <modules>
-      <module fileurl="file://$PROJECT_DIR$/.idea/path-planning-algorithms.iml" filepath="$PROJECT_DIR$/.idea/path-planning-algorithms.iml" />
+      <module fileurl="file://$PROJECT_DIR$/.idea/PathPlanning.iml" filepath="$PROJECT_DIR$/.idea/PathPlanning.iml" />
     </modules>
   </component>
 </project>

.idea/path-planning-algorithms.iml

@@ -1,11 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<module type="PYTHON_MODULE" version="4">
-  <component name="NewModuleRootManager">
-    <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="Python 3.7" jdkType="Python SDK" />
-    <orderEntry type="sourceFolder" forTests="false" />
-  </component>
-  <component name="PyDocumentationSettings">
-    <option name="myDocStringFormat" value="Plain" />
-  </component>
-</module>

CurvesGenerator/bezier_path.py

@@ -0,0 +1,145 @@
+"""
+bezier path
+
+author: Atsushi Sakai(@Atsushi_twi)
+modified: huiming zhou
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.special import comb
+import draw
+
+
+def calc_4points_bezier_path(sx, sy, syaw, gx, gy, gyaw, offset):
+
+    dist = np.hypot(sx - gx, sy - gy) / offset
+    control_points = np.array(
+        [[sx, sy],
+         [sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)],
+         [gx - dist * np.cos(gyaw), gy - dist * np.sin(gyaw)],
+         [gx, gy]])
+
+    path = calc_bezier_path(control_points, n_points=100)
+
+    return path, control_points
+
+
+def calc_bezier_path(control_points, n_points=100):
+    traj = []
+
+    for t in np.linspace(0, 1, n_points):
+        traj.append(bezier(t, control_points))
+
+    return np.array(traj)
+
+
+def Comb(n, i, t):
+    return comb(n, i) * t ** i * (1 - t) ** (n - i)
+
+
+def bezier(t, control_points):
+    n = len(control_points) - 1
+    return np.sum([Comb(n, i, t) * control_points[i] for i in range(n + 1)], axis=0)
+
+
+def bezier_derivatives_control_points(control_points, n_derivatives):
+    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):
+    return (dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** (3 / 2)
+
+
+def simulation():
+    sx = [-3, 0, 4, 6]
+    sy = [2, 0, 1.5, 6]
+
+    ratio = np.linspace(0, 1, 100)
+    pathx, pathy = [], []
+
+    for t in ratio:
+        x, y = [], []
+        for i in range(len(sx) - 1):
+            x.append(sx[i + 1] * t + sx[i] * (1 - t))
+            y.append(sy[i + 1] * t + sy[i] * (1 - t))
+
+        xx, yy = [], []
+        for i in range(len(x) - 1):
+            xx.append(x[i + 1] * t + x[i] * (1 - t))
+            yy.append(y[i + 1] * t + y[i] * (1 - t))
+
+        px = xx[1] * t + xx[0] * (1 - t)
+        py = yy[1] * t + yy[0] * (1 - t)
+        pathx.append(px)
+        pathy.append(py)
+
+        plt.cla()
+        plt.plot(sx, sy, linestyle='-', marker='o', color='dimgray', label="Control Points")
+        plt.plot(x, y, color='dodgerblue')
+        plt.plot(xx, yy, color='cyan')
+        plt.plot(pathx, pathy, color='darkorange', linewidth=2, label="Bezier Path")
+        plt.plot(px, py, marker='o')
+        plt.axis("equal")
+        plt.legend()
+        plt.title("Cubic Bezier Curve demo")
+        plt.grid(True)
+        plt.pause(0.001)
+
+    plt.show()
+
+
+def main():
+    sx, sy, syaw = 10.0, 1.0, np.deg2rad(180.0)
+    gx, gy, gyaw = 0.0, -3.0, np.deg2rad(-45.0)
+    offset = 3.0
+
+    path, control_points = calc_4points_bezier_path(sx, sy, syaw, gx, gy, gyaw, offset)
+
+    t = 0.8  # 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 curv
+    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] == sx, "path is invalid"
+    assert path.T[1][0] == sy, "path is invalid"
+    assert path.T[0][-1] == gx, "path is invalid"
+    assert path.T[1][-1] == gy, "path is invalid"
+
+    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)
+    draw.Arrow(sx, sy, syaw, 1, "darkorange")
+    draw.Arrow(gx, gy, gyaw, 1, "darkorange")
+    plt.grid(True)
+    plt.title("Bezier Path: from Atsushi's work")
+    ax.axis("equal")
+    plt.show()
+
+
+if __name__ == '__main__':
+    main()
+    # simulation()

CurvesGenerator/bspline_curve.py

@@ -0,0 +1,80 @@
+"""
+
+Path Planner with B-Spline
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+import scipy.interpolate as scipy_interpolate
+import cubic_spline as cs
+
+
+def approximate_b_spline_path(x, y, n_path_points, degree=3):
+    t = range(len(x))
+    x_tup = scipy_interpolate.splrep(t, x, k=degree)
+    y_tup = scipy_interpolate.splrep(t, y, k=degree)
+
+    x_list = list(x_tup)
+    x_list[1] = x + [0.0, 0.0, 0.0, 0.0]
+
+    y_list = list(y_tup)
+    y_list[1] = y + [0.0, 0.0, 0.0, 0.0]
+
+    ipl_t = np.linspace(0.0, len(x) - 1, n_path_points)
+    rx = scipy_interpolate.splev(ipl_t, x_list)
+    ry = scipy_interpolate.splev(ipl_t, y_list)
+
+    return rx, ry
+
+
+def interpolate_b_spline_path(x, y, n_path_points, degree=3):
+    ipl_t = np.linspace(0.0, len(x) - 1, len(x))
+    spl_i_x = scipy_interpolate.make_interp_spline(ipl_t, x, k=degree)
+    spl_i_y = scipy_interpolate.make_interp_spline(ipl_t, y, k=degree)
+
+    travel = np.linspace(0.0, len(x) - 1, n_path_points)
+    return spl_i_x(travel), spl_i_y(travel)
+
+
+def main():
+    print(__file__ + " start!!")
+    # way points
+    # way_point_x = [-1.0, 3.0, 4.0, 2.0, 1.0]
+    # way_point_y = [0.0, -3.0, 1.0, 1.0, 3.0]
+    way_point_x = [-2, 2.0, 3.5, 5.5, 6.0, 8.0]
+    way_point_y = [0, 2.7, -0.5, 0.5, 3.0, 4.0]
+
+    sp = cs.Spline2D(way_point_x, way_point_y)
+    s = np.arange(0, sp.s[-1], 0.1)
+
+    rx, ry, ryaw, rk = [], [], [], []
+    for i_s in s:
+        ix, iy = sp.calc_position(i_s)
+        rx.append(ix)
+        ry.append(iy)
+        ryaw.append(sp.calc_yaw(i_s))
+        rk.append(sp.calc_curvature(i_s))
+
+    n_course_point = 100  # sampling number
+    rax, ray = approximate_b_spline_path(way_point_x, way_point_y,
+                                         n_course_point)
+    rix, riy = interpolate_b_spline_path(way_point_x, way_point_y,
+                                         n_course_point)
+
+    # show results
+    plt.plot(way_point_x, way_point_y, '-og', label="Control Points")
+    plt.plot(rax, ray, '-r', label="Approximated B-Spline path")
+    plt.plot(rix, riy, '-b', label="Interpolated B-Spline path")
+    plt.plot(rx, ry, color='dimgray', label="Cubic Spline")
+    plt.grid(True)
+    plt.title("Curves Comparison")
+    plt.legend()
+    plt.axis("equal")
+    plt.show()
+
+
+if __name__ == '__main__':
+    main()

CurvesGenerator/cubic_spline.py

@@ -0,0 +1,262 @@
+#! /usr/bin/python
+# -*- coding: utf-8 -*-
+u"""
+Cubic Spline library on python
+
+author Atsushi Sakai
+
+usage: see test codes as below
+
+license: MIT
+"""
+import math
+import numpy as np
+import bisect
+
+
+class Spline:
+    u"""
+    Cubic Spline class
+    """
+
+    def __init__(self, x, y):
+        self.b, self.c, self.d, self.w = [], [], [], []
+
+        self.x = x
+        self.y = y
+
+        self.nx = len(x)  # dimension of x
+        h = np.diff(x)
+
+        # calc coefficient cBest
+        self.a = [iy for iy in y]
+
+        # calc coefficient cBest
+        A = self.__calc_A(h)
+        B = self.__calc_B(h)
+        self.c = np.linalg.solve(A, B)
+        #  print(self.c1)
+
+        # calc spline coefficient b and d
+        for i in range(self.nx - 1):
+            self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))
+            tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \
+                (self.c[i + 1] + 2.0 * self.c[i]) / 3.0
+            self.b.append(tb)
+
+    def calc(self, t):
+        u"""
+        Calc position
+
+        if t is outside of the input x, return None
+
+        """
+
+        if t < self.x[0]:
+            return None
+        elif t > self.x[-1]:
+            return None
+
+        i = self.__search_index(t)
+        dx = t - self.x[i]
+        result = self.a[i] + self.b[i] * dx + \
+            self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0
+
+        return result
+
+    def calcd(self, t):
+        u"""
+        Calc first derivative
+
+        if t is outside of the input x, return None
+        """
+
+        if t < self.x[0]:
+            return None
+        elif t > self.x[-1]:
+            return None
+
+        i = self.__search_index(t)
+        dx = t - self.x[i]
+        result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0
+        return result
+
+    def calcdd(self, t):
+        u"""
+        Calc second derivative
+        """
+
+        if t < self.x[0]:
+            return None
+        elif t > self.x[-1]:
+            return None
+
+        i = self.__search_index(t)
+        dx = t - self.x[i]
+        result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
+        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 cBest
+        """
+        A = np.zeros((self.nx, self.nx))
+        A[0, 0] = 1.0
+        for i in range(self.nx - 1):
+            if i != (self.nx - 2):
+                A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
+            A[i + 1, i] = h[i]
+            A[i, i + 1] = h[i]
+
+        A[0, 1] = 0.0
+        A[self.nx - 1, self.nx - 2] = 0.0
+        A[self.nx - 1, self.nx - 1] = 1.0
+        #  print(A)
+        return A
+
+    def __calc_B(self, h):
+        u"""
+        calc matrix B for spline coefficient cBest
+        """
+        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
+
+    """
+
+    def __init__(self, x, y):
+        self.s = self.__calc_s(x, y)
+        self.sx = Spline(self.s, x)
+        self.sy = Spline(self.s, y)
+
+    def __calc_s(self, x, y):
+        dx = np.diff(x)
+        dy = np.diff(y)
+        self.ds = [math.sqrt(idx ** 2 + idy ** 2)
+                   for (idx, idy) in zip(dx, dy)]
+        s = [0]
+        s.extend(np.cumsum(self.ds))
+        return s
+
+    def calc_position(self, s):
+        u"""
+        calc position
+        """
+        x = self.sx.calc(s)
+        y = self.sy.calc(s)
+
+        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)
+        return k
+
+    def calc_yaw(self, s):
+        u"""
+        calc yaw
+        """
+        dx = self.sx.calcd(s)
+        dy = self.sy.calcd(s)
+        yaw = math.atan2(dy, dx)
+        return yaw
+
+
+def calc_spline_course(x, y, ds=0.1):
+    sp = Spline2D(x, y)
+    s = np.arange(0, sp.s[-1], ds)
+
+    rx, ry, ryaw, rk = [], [], [], []
+    for i_s in s:
+        ix, iy = sp.calc_position(i_s)
+        rx.append(ix)
+        ry.append(iy)
+        ryaw.append(sp.calc_yaw(i_s))
+        rk.append(sp.calc_curvature(i_s))
+
+    return rx, ry, ryaw, rk, s
+
+
+def test_spline2d():
+    print("Spline 2D test")
+    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]
+
+    sp = Spline2D(x, y)
+    s = np.arange(0, sp.s[-1], 0.1)
+
+    rx, ry, ryaw, rk = [], [], [], []
+    for i_s in s:
+        ix, iy = sp.calc_position(i_s)
+        rx.append(ix)
+        ry.append(iy)
+        ryaw.append(sp.calc_yaw(i_s))
+        rk.append(sp.calc_curvature(i_s))
+
+    flg, ax = plt.subplots(1)
+    plt.plot(x, y, "xb", label="input")
+    plt.plot(rx, ry, "-r", label="spline")
+    plt.grid(True)
+    plt.axis("equal")
+    plt.xlabel("x[m]")
+    plt.ylabel("y[m]")
+    plt.legend()
+
+    flg, ax = plt.subplots(1)
+    plt.plot(s, [math.degrees(iyaw) for iyaw in ryaw], "-r", label="yaw")
+    plt.grid(True)
+    plt.legend()
+    plt.xlabel("line length[m]")
+    plt.ylabel("yaw angle[deg]")
+
+    flg, ax = plt.subplots(1)
+    plt.plot(s, rk, "-r", label="curvature")
+    plt.grid(True)
+    plt.legend()
+    plt.xlabel("line length[m]")
+    plt.ylabel("curvature [1/m]")
+
+    plt.show()
+
+
+def test_spline():
+    print("Spline test")
+    import matplotlib.pyplot as plt
+    x = [-0.5, 0.0, 0.5, 1.0, 1.5]
+    y = [3.2, 2.7, 6, 5, 6.5]
+
+    spline = Spline(x, y)
+    rx = np.arange(-2.0, 4, 0.01)
+    ry = [spline.calc(i) for i in rx]
+
+    plt.plot(x, y, "xb")
+    plt.plot(rx, ry, "-r")
+    plt.grid(True)
+    plt.axis("equal")
+    plt.show()
+
+
+if __name__ == '__main__':
+    test_spline()
+    # test_spline2d()

CurvesGenerator/draw.py

@@ -0,0 +1,66 @@
+import matplotlib.pyplot as plt
+import numpy as np
+PI = np.pi
+
+
+class Arrow:
+    def __init__(self, x, y, theta, L, c):
+        angle = np.deg2rad(30)
+        d = 0.5 * L
+        w = 2
+
+        x_start = x
+        y_start = y
+        x_end = x + L * np.cos(theta)
+        y_end = y + L * np.sin(theta)
+
+        theta_hat_L = theta + PI - angle
+        theta_hat_R = theta + PI + angle
+
+        x_hat_start = x_end
+        x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L)
+        x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R)
+
+        y_hat_start = y_end
+        y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L)
+        y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R)
+
+        plt.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w)
+        plt.plot([x_hat_start, x_hat_end_L],
+                 [y_hat_start, y_hat_end_L], color=c, linewidth=w)
+        plt.plot([x_hat_start, x_hat_end_R],
+                 [y_hat_start, y_hat_end_R], color=c, linewidth=w)
+
+
+class Car:
+    def __init__(self, x, y, yaw, w, L):
+        theta_B = PI + yaw
+
+        xB = x + L / 4 * np.cos(theta_B)
+        yB = y + L / 4 * np.sin(theta_B)
+
+        theta_BL = theta_B + PI / 2
+        theta_BR = theta_B - PI / 2
+
+        x_BL = xB + w / 2 * np.cos(theta_BL)        # Bottom-Left vertex
+        y_BL = yB + w / 2 * np.sin(theta_BL)
+        x_BR = xB + w / 2 * np.cos(theta_BR)        # Bottom-Right vertex
+        y_BR = yB + w / 2 * np.sin(theta_BR)
+
+        x_FL = x_BL + L * np.cos(yaw)               # Front-Left vertex
+        y_FL = y_BL + L * np.sin(yaw)
+        x_FR = x_BR + L * np.cos(yaw)               # Front-Right vertex
+        y_FR = y_BR + L * np.sin(yaw)
+
+        plt.plot([x_BL, x_BR, x_FR, x_FL, x_BL],
+                 [y_BL, y_BR, y_FR, y_FL, y_BL],
+                 linewidth=1, color='black')
+
+        Arrow(x, y, yaw, L / 2, 'black')
+        # plt.axis("equal")
+        # plt.show()
+
+
+if __name__ == '__main__':
+    # Arrow(-1, 2, 60)
+    Car(0, 0, 1, 2, 60)

CurvesGenerator/dubins_path.py

@@ -0,0 +1,351 @@
+"""
+Dubins Path
+"""
+
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.spatial.transform import Rotation as Rot
+import CurvesGenerator.draw as draw
+
+
+# class for PATH element
+class PATH:
+    def __init__(self, L, mode, x, y, yaw):
+        self.L = L  # total path length [float]
+        self.mode = mode  # type of each part of the path [string]
+        self.x = x  # final x positions [m]
+        self.y = y  # final y positions [m]
+        self.yaw = yaw  # final yaw angles [rad]
+
+
+# utils
+def pi_2_pi(theta):
+    while theta > math.pi:
+        theta -= 2.0 * math.pi
+
+    while theta < -math.pi:
+        theta += 2.0 * math.pi
+
+    return theta
+
+
+def mod2pi(theta):
+    return theta - 2.0 * math.pi * math.floor(theta / math.pi / 2.0)
+
+
+def LSL(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    p_lsl = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_a - sin_b)
+
+    if p_lsl < 0:
+        return None, None, None, ["L", "S", "L"]
+    else:
+        p_lsl = math.sqrt(p_lsl)
+
+    denominate = dist + sin_a - sin_b
+    t_lsl = mod2pi(-alpha + math.atan2(cos_b - cos_a, denominate))
+    q_lsl = mod2pi(beta - math.atan2(cos_b - cos_a, denominate))
+
+    return t_lsl, p_lsl, q_lsl, ["L", "S", "L"]
+
+
+def RSR(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    p_rsr = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_b - sin_a)
+
+    if p_rsr < 0:
+        return None, None, None, ["R", "S", "R"]
+    else:
+        p_rsr = math.sqrt(p_rsr)
+
+    denominate = dist - sin_a + sin_b
+    t_rsr = mod2pi(alpha - math.atan2(cos_a - cos_b, denominate))
+    q_rsr = mod2pi(-beta + math.atan2(cos_a - cos_b, denominate))
+
+    return t_rsr, p_rsr, q_rsr, ["R", "S", "R"]
+
+
+def LSR(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    p_lsr = -2 + dist ** 2 + 2 * cos_a_b + 2 * dist * (sin_a + sin_b)
+
+    if p_lsr < 0:
+        return None, None, None, ["L", "S", "R"]
+    else:
+        p_lsr = math.sqrt(p_lsr)
+
+    rec = math.atan2(-cos_a - cos_b, dist + sin_a + sin_b) - math.atan2(-2.0, p_lsr)
+    t_lsr = mod2pi(-alpha + rec)
+    q_lsr = mod2pi(-mod2pi(beta) + rec)
+
+    return t_lsr, p_lsr, q_lsr, ["L", "S", "R"]
+
+
+def RSL(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    p_rsl = -2 + dist ** 2 + 2 * cos_a_b - 2 * dist * (sin_a + sin_b)
+
+    if p_rsl < 0:
+        return None, None, None, ["R", "S", "L"]
+    else:
+        p_rsl = math.sqrt(p_rsl)
+
+    rec = math.atan2(cos_a + cos_b, dist - sin_a - sin_b) - math.atan2(2.0, p_rsl)
+    t_rsl = mod2pi(alpha - rec)
+    q_rsl = mod2pi(beta - rec)
+
+    return t_rsl, p_rsl, q_rsl, ["R", "S", "L"]
+
+
+def RLR(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_a - sin_b)) / 8.0
+
+    if abs(rec) > 1.0:
+        return None, None, None, ["R", "L", "R"]
+
+    p_rlr = mod2pi(2 * math.pi - math.acos(rec))
+    t_rlr = mod2pi(alpha - math.atan2(cos_a - cos_b, dist - sin_a + sin_b) + mod2pi(p_rlr / 2.0))
+    q_rlr = mod2pi(alpha - beta - t_rlr + mod2pi(p_rlr))
+
+    return t_rlr, p_rlr, q_rlr, ["R", "L", "R"]
+
+
+def LRL(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_b - sin_a)) / 8.0
+
+    if abs(rec) > 1.0:
+        return None, None, None, ["L", "R", "L"]
+
+    p_lrl = mod2pi(2 * math.pi - math.acos(rec))
+    t_lrl = mod2pi(-alpha - math.atan2(cos_a - cos_b, dist + sin_a - sin_b) + p_lrl / 2.0)
+    q_lrl = mod2pi(mod2pi(beta) - alpha - t_lrl + mod2pi(p_lrl))
+
+    return t_lrl, p_lrl, q_lrl, ["L", "R", "L"]
+
+
+def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):
+    if m == "S":
+        px[ind] = ox + l / maxc * math.cos(oyaw)
+        py[ind] = oy + l / maxc * math.sin(oyaw)
+        pyaw[ind] = oyaw
+    else:
+        ldx = math.sin(l) / maxc
+        if m == "L":
+            ldy = (1.0 - math.cos(l)) / maxc
+        elif m == "R":
+            ldy = (1.0 - math.cos(l)) / (-maxc)
+
+        gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy
+        gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy
+        px[ind] = ox + gdx
+        py[ind] = oy + gdy
+
+    if m == "L":
+        pyaw[ind] = oyaw + l
+    elif m == "R":
+        pyaw[ind] = oyaw - l
+
+    if l > 0.0:
+        directions[ind] = 1
+    else:
+        directions[ind] = -1
+
+    return px, py, pyaw, directions
+
+
+def generate_local_course(L, lengths, mode, maxc, step_size):
+    point_num = int(L / step_size) + len(lengths) + 3
+
+    px = [0.0 for _ in range(point_num)]
+    py = [0.0 for _ in range(point_num)]
+    pyaw = [0.0 for _ in range(point_num)]
+    directions = [0 for _ in range(point_num)]
+    ind = 1
+
+    if lengths[0] > 0.0:
+        directions[0] = 1
+    else:
+        directions[0] = -1
+
+    if lengths[0] > 0.0:
+        d = step_size
+    else:
+        d = -step_size
+
+    ll = 0.0
+
+    for m, l, i in zip(mode, lengths, range(len(mode))):
+        if l > 0.0:
+            d = step_size
+        else:
+            d = -step_size
+
+        ox, oy, oyaw = px[ind], py[ind], pyaw[ind]
+
+        ind -= 1
+        if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
+            pd = -d - ll
+        else:
+            pd = d - ll
+
+        while abs(pd) <= abs(l):
+            ind += 1
+            px, py, pyaw, directions = \
+                interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
+            pd += d
+
+        ll = l - pd - d  # calc remain length
+
+        ind += 1
+        px, py, pyaw, directions = \
+            interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
+
+    if len(px) <= 1:
+        return [], [], [], []
+
+    # remove unused data
+    while len(px) >= 1 and px[-1] == 0.0:
+        px.pop()
+        py.pop()
+        pyaw.pop()
+        directions.pop()
+
+    return px, py, pyaw, directions
+
+
+def planning_from_origin(gx, gy, gyaw, curv, step_size):
+    D = math.hypot(gx, gy)
+    d = D * curv
+
+    theta = mod2pi(math.atan2(gy, gx))
+    alpha = mod2pi(-theta)
+    beta = mod2pi(gyaw - theta)
+
+    planners = [LSL, RSR, LSR, RSL, RLR, LRL]
+
+    best_cost = float("inf")
+    bt, bp, bq, best_mode = None, None, None, None
+
+    for planner in planners:
+        t, p, q, mode = planner(alpha, beta, d)
+
+        if t is None:
+            continue
+
+        cost = (abs(t) + abs(p) + abs(q))
+        if best_cost > cost:
+            bt, bp, bq, best_mode = t, p, q, mode
+            best_cost = cost
+    lengths = [bt, bp, bq]
+
+    x_list, y_list, yaw_list, directions = generate_local_course(
+        sum(lengths), lengths, best_mode, curv, step_size)
+
+    return x_list, y_list, yaw_list, best_mode, best_cost
+
+
+def calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, curv, step_size=0.1):
+    gx = gx - sx
+    gy = gy - sy
+
+    l_rot = Rot.from_euler('z', syaw).as_dcm()[0:2, 0:2]
+    le_xy = np.stack([gx, gy]).T @ l_rot
+    le_yaw = gyaw - syaw
+
+    lp_x, lp_y, lp_yaw, mode, lengths = planning_from_origin(
+        le_xy[0], le_xy[1], le_yaw, curv, step_size)
+
+    rot = Rot.from_euler('z', -syaw).as_dcm()[0:2, 0:2]
+    converted_xy = np.stack([lp_x, lp_y]).T @ rot
+    x_list = converted_xy[:, 0] + sx
+    y_list = converted_xy[:, 1] + sy
+    yaw_list = [pi_2_pi(i_yaw + syaw) for i_yaw in lp_yaw]
+
+    return PATH(lengths, mode, x_list, y_list, yaw_list)
+
+
+def main():
+    # choose states pairs: (x, y, yaw)
+    # simulation-1
+    states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120),
+              (35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)]
+
+    # simulation-2
+    # states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),
+    #           (35, 10, 180), (32, -10, 180), (5, -12, 90)]
+
+    max_c = 0.25  # max curvature
+    path_x, path_y, yaw = [], [], []
+
+    for i in range(len(states) - 1):
+        s_x = states[i][0]
+        s_y = states[i][1]
+        s_yaw = np.deg2rad(states[i][2])
+        g_x = states[i + 1][0]
+        g_y = states[i + 1][1]
+        g_yaw = np.deg2rad(states[i + 1][2])
+
+        path_i = calc_dubins_path(s_x, s_y, s_yaw, g_x, g_y, g_yaw, max_c)
+
+        for x, y, iyaw in zip(path_i.x, path_i.y, path_i.yaw):
+            path_x.append(x)
+            path_y.append(y)
+            yaw.append(iyaw)
+
+    # animation
+    plt.ion()
+    plt.figure(1)
+
+    for i in range(len(path_x)):
+        plt.clf()
+        plt.plot(path_x, path_y, linewidth=1, color='gray')
+
+        for x, y, theta in states:
+            draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet')
+
+        draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3)
+
+        plt.axis("equal")
+        plt.title("Simulation of Dubins Path")
+        plt.axis([-10, 42, -20, 20])
+        plt.draw()
+        plt.pause(0.001)
+
+    plt.pause(1)
+
+
+if __name__ == '__main__':
+    main()

CurvesGenerator/quartic_polynomial.py

@@ -0,0 +1,43 @@
+"""
+Quartic Polynomial
+"""
+
+import numpy as np
+
+
+class QuarticPolynomial:
+    def __init__(self, x0, v0, a0, v1, a1, T):
+        A = np.array([[3 * T ** 2, 4 * T ** 3],
+                      [6 * T, 12 * T ** 2]])
+        b = np.array([v1 - v0 - a0 * T,
+                      a1 - a0])
+        X = np.linalg.solve(A, b)
+
+        self.a0 = x0
+        self.a1 = v0
+        self.a2 = a0 / 2.0
+        self.a3 = X[0]
+        self.a4 = X[1]
+
+    def calc_xt(self, t):
+        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
+             self.a3 * t ** 3 + self.a4 * t ** 4
+
+        return xt
+
+    def calc_dxt(self, t):
+        xt = self.a1 + 2 * self.a2 * t + \
+             3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3
+
+        return xt
+
+    def calc_ddxt(self, t):
+        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2
+
+        return xt
+
+    def calc_dddxt(self, t):
+        xt = 6 * self.a3 + 24 * self.a4 * t
+
+        return xt
+

CurvesGenerator/quintic_polynomial.py

@@ -0,0 +1,143 @@
+"""
+Quintic Polynomial
+"""
+
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+
+import draw
+
+
+class QuinticPolynomial:
+    def __init__(self, x0, v0, a0, x1, v1, a1, T):
+        A = np.array([[T ** 3, T ** 4, T ** 5],
+                      [3 * T ** 2, 4 * T ** 3, 5 * T ** 4],
+                      [6 * T, 12 * T ** 2, 20 * T ** 3]])
+        b = np.array([x1 - x0 - v0 * T - a0 * T ** 2 / 2,
+                      v1 - v0 - a0 * T,
+                      a1 - a0])
+        X = np.linalg.solve(A, b)
+
+        self.a0 = x0
+        self.a1 = v0
+        self.a2 = a0 / 2.0
+        self.a3 = X[0]
+        self.a4 = X[1]
+        self.a5 = X[2]
+
+    def calc_xt(self, t):
+        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
+                self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5
+
+        return xt
+
+    def calc_dxt(self, t):
+        dxt = self.a1 + 2 * self.a2 * t + \
+            3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4
+
+        return dxt
+
+    def calc_ddxt(self, t):
+        ddxt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3
+
+        return ddxt
+
+    def calc_dddxt(self, t):
+        dddxt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2
+
+        return dddxt
+
+
+class Trajectory:
+    def __init__(self):
+        self.t = []
+        self.x = []
+        self.y = []
+        self.yaw = []
+        self.v = []
+        self.a = []
+        self.jerk = []
+
+
+def simulation():
+    sx, sy, syaw, sv, sa = 10.0, 10.0, np.deg2rad(10.0), 1.0, 0.1
+    gx, gy, gyaw, gv, ga = 30.0, -10.0, np.deg2rad(180.0), 1.0, 0.1
+
+    MAX_ACCEL = 1.0  # max accel [m/s2]
+    MAX_JERK = 0.5  # max jerk [m/s3]
+    dt = 0.1  # T tick [s]
+
+    MIN_T = 5
+    MAX_T = 100
+    T_STEP = 5
+
+    sv_x = sv * math.cos(syaw)
+    sv_y = sv * math.sin(syaw)
+    gv_x = gv * math.cos(gyaw)
+    gv_y = gv * math.sin(gyaw)
+
+    sa_x = sa * math.cos(syaw)
+    sa_y = sa * math.sin(syaw)
+    ga_x = ga * math.cos(gyaw)
+    ga_y = ga * math.sin(gyaw)
+
+    path = Trajectory()
+
+    for T in np.arange(MIN_T, MAX_T, T_STEP):
+        path = Trajectory()
+        xqp = QuinticPolynomial(sx, sv_x, sa_x, gx, gv_x, ga_x, T)
+        yqp = QuinticPolynomial(sy, sv_y, sa_y, gy, gv_y, ga_y, T)
+
+        for t in np.arange(0.0, T + dt, dt):
+            path.t.append(t)
+            path.x.append(xqp.calc_xt(t))
+            path.y.append(yqp.calc_xt(t))
+
+            vx = xqp.calc_dxt(t)
+            vy = yqp.calc_dxt(t)
+            path.v.append(np.hypot(vx, vy))
+            path.yaw.append(math.atan2(vy, vx))
+
+            ax = xqp.calc_ddxt(t)
+            ay = yqp.calc_ddxt(t)
+            a = np.hypot(ax, ay)
+
+            if len(path.v) >= 2 and path.v[-1] - path.v[-2] < 0.0:
+                a *= -1
+            path.a.append(a)
+
+            jx = xqp.calc_dddxt(t)
+            jy = yqp.calc_dddxt(t)
+            j = np.hypot(jx, jy)
+
+            if len(path.a) >= 2 and path.a[-1] - path.a[-2] < 0.0:
+                j *= -1
+            path.jerk.append(j)
+
+        if max(np.abs(path.a)) <= MAX_ACCEL and max(np.abs(path.jerk)) <= MAX_JERK:
+            break
+
+    print("t_len: ", path.t, "s")
+    print("max_v: ", max(path.v), "m/s")
+    print("max_a: ", max(np.abs(path.a)), "m/s2")
+    print("max_jerk: ", max(np.abs(path.jerk)), "m/s3")
+
+    for i in range(len(path.t)):
+        plt.cla()
+        plt.gcf().canvas.mpl_connect('key_release_event',
+                                     lambda event: [exit(0) if event.key == 'escape' else None])
+        plt.axis("equal")
+        plt.plot(path.x, path.y, linewidth=2, color='gray')
+        draw.Car(sx, sy, syaw, 1.5, 3)
+        draw.Car(gx, gy, gyaw, 1.5, 3)
+        draw.Car(path.x[i], path.y[i], path.yaw[i], 1.5, 3)
+        plt.title("Quintic Polynomial Curves")
+        plt.grid(True)
+        plt.pause(0.001)
+
+    plt.show()
+
+
+if __name__ == '__main__':
+    simulation()

CurvesGenerator/reeds_shepp.py

@@ -0,0 +1,720 @@
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+
+import draw
+
+# parameters initiation
+STEP_SIZE = 0.2
+MAX_LENGTH = 1000.0
+PI = math.pi
+
+
+# class for PATH element
+class PATH:
+    def __init__(self, lengths, ctypes, L, x, y, yaw, directions):
+        self.lengths = lengths  # lengths of each part of path (+: forward, -: backward) [float]
+        self.ctypes = ctypes  # type of each part of the path [string]
+        self.L = L  # total path length [float]
+        self.x = x  # final x positions [m]
+        self.y = y  # final y positions [m]
+        self.yaw = yaw  # final yaw angles [rad]
+        self.directions = directions  # forward: 1, backward:-1
+
+
+def calc_optimal_path(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):
+    paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=step_size)
+
+    minL = paths[0].L
+    mini = 0
+
+    for i in range(len(paths)):
+        if paths[i].L <= minL:
+            minL, mini = paths[i].L, i
+
+    return paths[mini]
+
+
+def calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):
+    q0 = [sx, sy, syaw]
+    q1 = [gx, gy, gyaw]
+
+    paths = generate_path(q0, q1, maxc)
+
+    for path in paths:
+        x, y, yaw, directions = \
+            generate_local_course(path.L, path.lengths,
+                                  path.ctypes, maxc, step_size * maxc)
+
+        # convert global coordinate
+        path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for (ix, iy) in zip(x, y)]
+        path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for (ix, iy) in zip(x, y)]
+        path.yaw = [pi_2_pi(iyaw + q0[2]) for iyaw in yaw]
+        path.directions = directions
+        path.lengths = [l / maxc for l in path.lengths]
+        path.L = path.L / maxc
+
+    return paths
+
+
+def set_path(paths, lengths, ctypes):
+    path = PATH([], [], 0.0, [], [], [], [])
+    path.ctypes = ctypes
+    path.lengths = lengths
+
+    # check same path exist
+    for path_e in paths:
+        if path_e.ctypes == path.ctypes:
+            if sum([x - y for x, y in zip(path_e.lengths, path.lengths)]) <= 0.01:
+                return paths  # not insert path
+
+    path.L = sum([abs(i) for i in lengths])
+
+    if path.L >= MAX_LENGTH:
+        return paths
+
+    assert path.L >= 0.01
+    paths.append(path)
+
+    return paths
+
+
+def LSL(x, y, phi):
+    u, t = R(x - math.sin(phi), y - 1.0 + math.cos(phi))
+
+    if t >= 0.0:
+        v = M(phi - t)
+        if v >= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LSR(x, y, phi):
+    u1, t1 = R(x + math.sin(phi), y - 1.0 - math.cos(phi))
+    u1 = u1 ** 2
+
+    if u1 >= 4.0:
+        u = math.sqrt(u1 - 4.0)
+        theta = math.atan2(2.0, u)
+        t = M(t1 + theta)
+        v = M(t - phi)
+
+        if t >= 0.0 and v >= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LRL(x, y, phi):
+    u1, t1 = R(x - math.sin(phi), y - 1.0 + math.cos(phi))
+
+    if u1 <= 4.0:
+        u = -2.0 * math.asin(0.25 * u1)
+        t = M(t1 + 0.5 * u + PI)
+        v = M(phi - t + u)
+
+        if t >= 0.0 and u <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def SCS(x, y, phi, paths):
+    flag, t, u, v = SLS(x, y, phi)
+
+    if flag:
+        paths = set_path(paths, [t, u, v], ["S", "L", "S"])
+
+    flag, t, u, v = SLS(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["S", "R", "S"])
+
+    return paths
+
+
+def SLS(x, y, phi):
+    phi = M(phi)
+
+    if y > 0.0 and 0.0 < phi < PI * 0.99:
+        xd = -y / math.tan(phi) + x
+        t = xd - math.tan(phi / 2.0)
+        u = phi
+        v = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
+        return True, t, u, v
+    elif y < 0.0 and 0.0 < phi < PI * 0.99:
+        xd = -y / math.tan(phi) + x
+        t = xd - math.tan(phi / 2.0)
+        u = phi
+        v = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
+        return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CSC(x, y, phi, paths):
+    flag, t, u, v = LSL(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["L", "S", "L"])
+
+    flag, t, u, v = LSL(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["L", "S", "L"])
+
+    flag, t, u, v = LSL(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["R", "S", "R"])
+
+    flag, t, u, v = LSL(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"])
+
+    flag, t, u, v = LSR(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["L", "S", "R"])
+
+    flag, t, u, v = LSR(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["L", "S", "R"])
+
+    flag, t, u, v = LSR(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["R", "S", "L"])
+
+    flag, t, u, v = LSR(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["R", "S", "L"])
+
+    return paths
+
+
+def CCC(x, y, phi, paths):
+    flag, t, u, v = LRL(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["L", "R", "L"])
+
+    flag, t, u, v = LRL(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["L", "R", "L"])
+
+    flag, t, u, v = LRL(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["R", "L", "R"])
+
+    flag, t, u, v = LRL(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["R", "L", "R"])
+
+    # backwards
+    xb = x * math.cos(phi) + y * math.sin(phi)
+    yb = x * math.sin(phi) - y * math.cos(phi)
+
+    flag, t, u, v = LRL(xb, yb, phi)
+    if flag:
+        paths = set_path(paths, [v, u, t], ["L", "R", "L"])
+
+    flag, t, u, v = LRL(-xb, yb, -phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, -t], ["L", "R", "L"])
+
+    flag, t, u, v = LRL(xb, -yb, -phi)
+    if flag:
+        paths = set_path(paths, [v, u, t], ["R", "L", "R"])
+
+    flag, t, u, v = LRL(-xb, -yb, phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, -t], ["R", "L", "R"])
+
+    return paths
+
+
+def calc_tauOmega(u, v, xi, eta, phi):
+    delta = M(u - v)
+    A = math.sin(u) - math.sin(delta)
+    B = math.cos(u) - math.cos(delta) - 1.0
+
+    t1 = math.atan2(eta * A - xi * B, xi * A + eta * B)
+    t2 = 2.0 * (math.cos(delta) - math.cos(v) - math.cos(u)) + 3.0
+
+    if t2 < 0:
+        tau = M(t1 + PI)
+    else:
+        tau = M(t1)
+
+    omega = M(tau - u + v - phi)
+
+    return tau, omega
+
+
+def LRLRn(x, y, phi):
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho = 0.25 * (2.0 + math.sqrt(xi * xi + eta * eta))
+
+    if rho <= 1.0:
+        u = math.acos(rho)
+        t, v = calc_tauOmega(u, -u, xi, eta, phi)
+        if t >= 0.0 and v <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LRLRp(x, y, phi):
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho = (20.0 - xi * xi - eta * eta) / 16.0
+
+    if 0.0 <= rho <= 1.0:
+        u = -math.acos(rho)
+        if u >= -0.5 * PI:
+            t, v = calc_tauOmega(u, u, xi, eta, phi)
+            if t >= 0.0 and v >= 0.0:
+                return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CCCC(x, y, phi, paths):
+    flag, t, u, v = LRLRn(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, -u, v], ["L", "R", "L", "R"])
+
+    flag, t, u, v = LRLRn(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, u, -v], ["L", "R", "L", "R"])
+
+    flag, t, u, v = LRLRn(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, -u, v], ["R", "L", "R", "L"])
+
+    flag, t, u, v = LRLRn(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, u, -v], ["R", "L", "R", "L"])
+
+    flag, t, u, v = LRLRp(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, u, v], ["L", "R", "L", "R"])
+
+    flag, t, u, v = LRLRp(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -u, -v], ["L", "R", "L", "R"])
+
+    flag, t, u, v = LRLRp(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, u, v], ["R", "L", "R", "L"])
+
+    flag, t, u, v = LRLRp(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -u, -v], ["R", "L", "R", "L"])
+
+    return paths
+
+
+def LRSR(x, y, phi):
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho, theta = R(-eta, xi)
+
+    if rho >= 2.0:
+        t = theta
+        u = 2.0 - rho
+        v = M(t + 0.5 * PI - phi)
+        if t >= 0.0 and u <= 0.0 and v <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LRSL(x, y, phi):
+    xi = x - math.sin(phi)
+    eta = y - 1.0 + math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho >= 2.0:
+        r = math.sqrt(rho * rho - 4.0)
+        u = 2.0 - r
+        t = M(theta + math.atan2(r, -2.0))
+        v = M(phi - 0.5 * PI - t)
+        if t >= 0.0 and u <= 0.0 and v <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CCSC(x, y, phi, paths):
+    flag, t, u, v = LRSL(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["L", "R", "S", "L"])
+
+    flag, t, u, v = LRSL(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["L", "R", "S", "L"])
+
+    flag, t, u, v = LRSL(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "L", "S", "R"])
+
+    flag, t, u, v = LRSL(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "L", "S", "R"])
+
+    flag, t, u, v = LRSR(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["L", "R", "S", "R"])
+
+    flag, t, u, v = LRSR(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["L", "R", "S", "R"])
+
+    flag, t, u, v = LRSR(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "L", "S", "L"])
+
+    flag, t, u, v = LRSR(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "L", "S", "L"])
+
+    # backwards
+    xb = x * math.cos(phi) + y * math.sin(phi)
+    yb = x * math.sin(phi) - y * math.cos(phi)
+
+    flag, t, u, v = LRSL(xb, yb, phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["L", "S", "R", "L"])
+
+    flag, t, u, v = LRSL(-xb, yb, -phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["L", "S", "R", "L"])
+
+    flag, t, u, v = LRSL(xb, -yb, -phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "L", "R"])
+
+    flag, t, u, v = LRSL(-xb, -yb, phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "L", "R"])
+
+    flag, t, u, v = LRSR(xb, yb, phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "R", "L"])
+
+    flag, t, u, v = LRSR(-xb, yb, -phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "R", "L"])
+
+    flag, t, u, v = LRSR(xb, -yb, -phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["L", "S", "L", "R"])
+
+    flag, t, u, v = LRSR(-xb, -yb, phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["L", "S", "L", "R"])
+
+    return paths
+
+
+def LRSLR(x, y, phi):
+    # formula 8.11 *** TYPO IN PAPER ***
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho >= 2.0:
+        u = 4.0 - math.sqrt(rho * rho - 4.0)
+        if u <= 0.0:
+            t = M(math.atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta))
+            v = M(t - phi)
+
+            if t >= 0.0 and v >= 0.0:
+                return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CCSCC(x, y, phi, paths):
+    flag, t, u, v = LRSLR(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["L", "R", "S", "L", "R"])
+
+    flag, t, u, v = LRSLR(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["L", "R", "S", "L", "R"])
+
+    flag, t, u, v = LRSLR(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["R", "L", "S", "R", "L"])
+
+    flag, t, u, v = LRSLR(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["R", "L", "S", "R", "L"])
+
+    return paths
+
+
+def generate_local_course(L, lengths, mode, maxc, step_size):
+    point_num = int(L / step_size) + len(lengths) + 3
+
+    px = [0.0 for _ in range(point_num)]
+    py = [0.0 for _ in range(point_num)]
+    pyaw = [0.0 for _ in range(point_num)]
+    directions = [0 for _ in range(point_num)]
+    ind = 1
+
+    if lengths[0] > 0.0:
+        directions[0] = 1
+    else:
+        directions[0] = -1
+
+    if lengths[0] > 0.0:
+        d = step_size
+    else:
+        d = -step_size
+
+    pd = d
+    ll = 0.0
+
+    for m, l, i in zip(mode, lengths, range(len(mode))):
+        if l > 0.0:
+            d = step_size
+        else:
+            d = -step_size
+
+        ox, oy, oyaw = px[ind], py[ind], pyaw[ind]
+
+        ind -= 1
+        if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
+            pd = -d - ll
+        else:
+            pd = d - ll
+
+        while abs(pd) <= abs(l):
+            ind += 1
+            px, py, pyaw, directions = \
+                interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
+            pd += d
+
+        ll = l - pd - d  # calc remain length
+
+        ind += 1
+        px, py, pyaw, directions = \
+            interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
+
+    # remove unused data
+    while px[-1] == 0.0:
+        px.pop()
+        py.pop()
+        pyaw.pop()
+        directions.pop()
+
+    return px, py, pyaw, directions
+
+
+def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):
+    if m == "S":
+        px[ind] = ox + l / maxc * math.cos(oyaw)
+        py[ind] = oy + l / maxc * math.sin(oyaw)
+        pyaw[ind] = oyaw
+    else:
+        ldx = math.sin(l) / maxc
+        if m == "L":
+            ldy = (1.0 - math.cos(l)) / maxc
+        elif m == "R":
+            ldy = (1.0 - math.cos(l)) / (-maxc)
+
+        gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy
+        gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy
+        px[ind] = ox + gdx
+        py[ind] = oy + gdy
+
+    if m == "L":
+        pyaw[ind] = oyaw + l
+    elif m == "R":
+        pyaw[ind] = oyaw - l
+
+    if l > 0.0:
+        directions[ind] = 1
+    else:
+        directions[ind] = -1
+
+    return px, py, pyaw, directions
+
+
+def generate_path(q0, q1, maxc):
+    dx = q1[0] - q0[0]
+    dy = q1[1] - q0[1]
+    dth = q1[2] - q0[2]
+    c = math.cos(q0[2])
+    s = math.sin(q0[2])
+    x = (c * dx + s * dy) * maxc
+    y = (-s * dx + c * dy) * maxc
+
+    paths = []
+    paths = SCS(x, y, dth, paths)
+    paths = CSC(x, y, dth, paths)
+    paths = CCC(x, y, dth, paths)
+    paths = CCCC(x, y, dth, paths)
+    paths = CCSC(x, y, dth, paths)
+    paths = CCSCC(x, y, dth, paths)
+
+    return paths
+
+
+# utils
+def pi_2_pi(theta):
+    while theta > PI:
+        theta -= 2.0 * PI
+
+    while theta < -PI:
+        theta += 2.0 * PI
+
+    return theta
+
+
+def R(x, y):
+    """
+    Return the polar coordinates (r, theta) of the point (x, y)
+    """
+    r = math.hypot(x, y)
+    theta = math.atan2(y, x)
+
+    return r, theta
+
+
+def M(theta):
+    """
+    Regulate theta to -pi <= theta < pi
+    """
+    phi = theta % (2.0 * PI)
+
+    if phi < -PI:
+        phi += 2.0 * PI
+    if phi > PI:
+        phi -= 2.0 * PI
+
+    return phi
+
+
+def get_label(path):
+    label = ""
+
+    for m, l in zip(path.ctypes, path.lengths):
+        label = label + m
+        if l > 0.0:
+            label = label + "+"
+        else:
+            label = label + "-"
+
+    return label
+
+
+def calc_curvature(x, y, yaw, directions):
+    c, ds = [], []
+
+    for i in range(1, len(x) - 1):
+        dxn = x[i] - x[i - 1]
+        dxp = x[i + 1] - x[i]
+        dyn = y[i] - y[i - 1]
+        dyp = y[i + 1] - y[i]
+        dn = math.hypot(dxn, dyn)
+        dp = math.hypot(dxp, dyp)
+        dx = 1.0 / (dn + dp) * (dp / dn * dxn + dn / dp * dxp)
+        ddx = 2.0 / (dn + dp) * (dxp / dp - dxn / dn)
+        dy = 1.0 / (dn + dp) * (dp / dn * dyn + dn / dp * dyp)
+        ddy = 2.0 / (dn + dp) * (dyp / dp - dyn / dn)
+        curvature = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
+        d = (dn + dp) / 2.0
+
+        if np.isnan(curvature):
+            curvature = 0.0
+
+        if directions[i] <= 0.0:
+            curvature = -curvature
+
+        if len(c) == 0:
+            ds.append(d)
+            c.append(curvature)
+
+        ds.append(d)
+        c.append(curvature)
+
+    ds.append(ds[-1])
+    c.append(c[-1])
+
+    return c, ds
+
+
+def check_path(sx, sy, syaw, gx, gy, gyaw, maxc):
+    paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc)
+
+    assert len(paths) >= 1
+
+    for path in paths:
+        assert abs(path.x[0] - sx) <= 0.01
+        assert abs(path.y[0] - sy) <= 0.01
+        assert abs(path.yaw[0] - syaw) <= 0.01
+        assert abs(path.x[-1] - gx) <= 0.01
+        assert abs(path.y[-1] - gy) <= 0.01
+        assert abs(path.yaw[-1] - gyaw) <= 0.01
+
+        # course distance check
+        d = [math.hypot(dx, dy)
+             for dx, dy in zip(np.diff(path.x[0:len(path.x) - 1]),
+                               np.diff(path.y[0:len(path.y) - 1]))]
+
+        for i in range(len(d)):
+            assert abs(d[i] - STEP_SIZE) <= 0.001
+
+
+def main():
+    # choose states pairs: (x, y, yaw)
+    # simulation-1
+    # states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120),
+    #           (35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)]
+
+    # simulation-2
+    states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),
+              (35, 10, 180), (32, -10, 180), (5, -12, 90)]
+
+    max_c = 0.1  # max curvature
+    path_x, path_y, yaw = [], [], []
+
+    for i in range(len(states) - 1):
+        s_x = states[i][0]
+        s_y = states[i][1]
+        s_yaw = np.deg2rad(states[i][2])
+        g_x = states[i + 1][0]
+        g_y = states[i + 1][1]
+        g_yaw = np.deg2rad(states[i + 1][2])
+
+        path_i = calc_optimal_path(s_x, s_y, s_yaw,
+                                   g_x, g_y, g_yaw, max_c)
+
+        path_x += path_i.x
+        path_y += path_i.y
+        yaw += path_i.yaw
+
+    # animation
+    plt.ion()
+    plt.figure(1)
+
+    for i in range(len(path_x)):
+        plt.clf()
+        plt.plot(path_x, path_y, linewidth=1, color='gray')
+
+        for x, y, theta in states:
+            draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet')
+
+        draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3)
+
+        plt.axis("equal")
+        plt.title("Simulation of Reeds-Shepp Curves")
+        plt.axis([-10, 42, -20, 20])
+        plt.draw()
+        plt.pause(0.001)
+
+    plt.pause(1)
+
+
+if __name__ == '__main__':
+    main()

Model-free Control/.idea/.gitignore

@@ -1,3 +0,0 @@
-
-# Default ignored files
-/workspace.xml

Model-free Control/.idea/Model-free Control.iml

@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<module type="PYTHON_MODULE" version="4">
-  <component name="NewModuleRootManager">
-    <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="Python 3.7 (Search-based Planning)" jdkType="Python SDK" />
-    <orderEntry type="sourceFolder" forTests="false" />
-  </component>
-</module>

Model-free Control/.idea/dictionaries/Huiming_Zhou.xml

@@ -1,7 +0,0 @@
-<component name="ProjectDictionaryState">
-  <dictionary name="Huiming Zhou">
-    <words>
-      <w>sarsa</w>
-    </words>
-  </dictionary>
-</component>

Model-free Control/.idea/inspectionProfiles/profiles_settings.xml

@@ -1,6 +0,0 @@
-<component name="InspectionProjectProfileManager">
-  <settings>
-    <option name="USE_PROJECT_PROFILE" value="false" />
-    <version value="1.0" />
-  </settings>
-</component>

Model-free Control/.idea/misc.xml

@@ -1,4 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7 (Search-based Planning)" project-jdk-type="Python SDK" />
-</project>

Model-free Control/.idea/modules.xml

@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectModuleManager">
-    <modules>
-      <module fileurl="file://$PROJECT_DIR$/.idea/Model-free Control.iml" filepath="$PROJECT_DIR$/.idea/Model-free Control.iml" />
-    </modules>
-  </component>
-</project>

Model-free Control/.idea/vcs.xml

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="VcsDirectoryMappings">
-    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
-  </component>
-</project>

Model-free Control/Q-learning.py

@@ -1,166 +0,0 @@
-import env
-import plotting
-import motion_model
-
-import numpy as np
-
-
-class QLEARNING:
-    def __init__(self, x_start, x_goal):
-        self.xI, self.xG = x_start, x_goal
-        self.M = 500  # iteration numbers
-        self.gamma = 0.9  # discount factor
-        self.alpha = 0.5
-        self.epsilon = 0.1
-
-        self.env = env.Env(self.xI, self.xG)
-        self.motion = motion_model.Motion_model(self.xI, self.xG)
-        self.plotting = plotting.Plotting(self.xI, self.xG)
-
-        self.u_set = self.env.motions  # feasible input set
-        self.stateSpace = self.env.stateSpace  # state space
-        self.obs = self.env.obs_map()  # position of obstacles
-        self.lose = self.env.lose_map()  # position of lose states
-
-        self.name1 = "SARSA, M=" + str(self.M)
-
-        [self.value, self.policy] = self.Monte_Carlo(self.xI, self.xG)
-        self.path = self.extract_path(self.xI, self.xG, self.policy)
-        self.plotting.animation(self.path, self.name1)
-
-    def Monte_Carlo(self, xI, xG):
-        """
-        Monte_Carlo experiments
-
-        :return: Q_table, policy
-        """
-
-        Q_table = self.table_init()  # Q_table initialization
-        policy = {}  # policy table
-
-        for k in range(self.M):  # iterations
-            x = self.state_init()  # initial state
-            while x != xG:  # stop condition
-                u = self.epsilon_greedy(int(np.argmax(Q_table[x])), self.epsilon)  # epsilon_greedy policy
-                x_next = self.move_next(x, self.u_set[u])  # next state
-                reward = self.env.get_reward(x_next)  # reward observed
-                Q_table[x][u] = (1 - self.alpha) * Q_table[x][u] + \
-                                self.alpha * (reward + self.gamma * max(Q_table[x_next]))
-                x = x_next
-
-        for x in Q_table:
-            policy[x] = int(np.argmax(Q_table[x]))  # extract policy
-
-        return Q_table, policy
-
-    def table_init(self):
-        """
-        Initialize Q_table: Q(s, a)
-        :return: Q_table
-        """
-
-        Q_table = {}
-
-        for x in self.stateSpace:
-            u = []
-            if x not in self.obs:
-                for k in range(len(self.u_set)):
-                    if x == self.xG:
-                        u.append(0)
-                    else:
-                        u.append(np.random.random_sample())
-                    Q_table[x] = u
-        return Q_table
-
-    def state_init(self):
-        """
-        initialize a starting state
-        :return: starting state
-        """
-        while True:
-            i = np.random.randint(0, self.env.x_range - 1)
-            j = np.random.randint(0, self.env.y_range - 1)
-            if (i, j) not in self.obs:
-                return (i, j)
-
-    def epsilon_greedy(self, u, error):
-        """
-        generate a policy using epsilon_greedy algorithm
-
-        :param u: original input
-        :param error: epsilon value
-        :return: epsilon policy
-        """
-
-        if np.random.random_sample() < 3 / 4 * error:
-            u_e = u
-            while u_e == u:
-                p = np.random.random_sample()
-                if p < 0.25:
-                    u_e = 0
-                elif p < 0.5:
-                    u_e = 1
-                elif p < 0.75:
-                    u_e = 2
-                else:
-                    u_e = 3
-            return u_e
-        return u
-
-    def move_next(self, x, u):
-        """
-        get next state.
-
-        :param x: current state
-        :param u: input
-        :return: next state
-        """
-
-        x_next = (x[0] + u[0], x[1] + u[1])
-        if x_next in self.obs:
-            return x
-        return x_next
-
-    def extract_path(self, xI, xG, policy):
-        """
-        extract path from converged policy.
-
-        :param xI: starting state
-        :param xG: goal states
-        :param policy: converged policy
-        :return: path
-        """
-
-        x, path = xI, [xI]
-        while x != xG:
-            u = self.u_set[policy[x]]
-            x_next = (x[0] + u[0], x[1] + u[1])
-            if x_next in self.obs:
-                print("Collision! Please run again!")
-                break
-            else:
-                path.append(x_next)
-                x = x_next
-        return path
-
-    def message(self):
-        """
-        print important message.
-
-        :param count: iteration numbers
-        :return: print
-        """
-
-        print("starting state: ", self.xI)
-        print("goal state: ", self.xG)
-        print("iteration numbers: ", self.M)
-        print("discount factor: ", self.gamma)
-        print("epsilon error: ", self.epsilon)
-        print("alpha: ", self.alpha)
-
-
-if __name__ == '__main__':
-    x_Start = (1, 1)
-    x_Goal = (12, 1)
-
-    Q_CALL = QLEARNING(x_Start, x_Goal)

Model-free Control/Sarsa.py

@@ -1,167 +0,0 @@
-import env
-import plotting
-import motion_model
-
-import numpy as np
-
-
-class SARSA:
-    def __init__(self, x_start, x_goal):
-        self.xI, self.xG = x_start, x_goal
-        self.M = 500  # iteration numbers
-        self.gamma = 0.9  # discount factor
-        self.alpha = 0.5
-        self.epsilon = 0.1
-
-        self.env = env.Env(self.xI, self.xG)
-        self.motion = motion_model.Motion_model(self.xI, self.xG)
-        self.plotting = plotting.Plotting(self.xI, self.xG)
-
-        self.u_set = self.env.motions  # feasible input set
-        self.stateSpace = self.env.stateSpace  # state space
-        self.obs = self.env.obs_map()  # position of obstacles
-        self.lose = self.env.lose_map()  # position of lose states
-
-        self.name1 = "Q-learning, M=" + str(self.M)
-
-        [self.value, self.policy] = self.Monte_Carlo(self.xI, self.xG)
-        self.path = self.extract_path(self.xI, self.xG, self.policy)
-        self.plotting.animation(self.path, self.name1)
-
-    def Monte_Carlo(self, xI, xG):
-        """
-        Monte_Carlo experiments
-
-        :return: Q_table, policy
-        """
-
-        Q_table = self.table_init()  # Q_table initialization
-        policy = {}  # policy table
-
-        for k in range(self.M):  # iterations
-            x = self.state_init()  # initial state
-            u = self.epsilon_greedy(int(np.argmax(Q_table[x])), self.epsilon)
-            while x != xG:  # stop condition
-                x_next = self.move_next(x, self.u_set[u])  # next state
-                reward = self.env.get_reward(x_next)  # reward observed
-                u_next = self.epsilon_greedy(int(np.argmax(Q_table[x_next])), self.epsilon)
-                Q_table[x][u] = (1 - self.alpha) * Q_table[x][u] + \
-                                self.alpha * (reward + self.gamma * Q_table[x_next][u_next])
-                x, u = x_next, u_next
-
-        for x in Q_table:
-            policy[x] = int(np.argmax(Q_table[x]))  # extract policy
-
-        return Q_table, policy
-
-    def table_init(self):
-        """
-        Initialize Q_table: Q(s, a)
-        :return: Q_table
-        """
-
-        Q_table = {}
-
-        for x in self.stateSpace:
-            u = []
-            if x not in self.obs:
-                for k in range(len(self.u_set)):
-                    if x == self.xG:
-                        u.append(0)
-                    else:
-                        u.append(np.random.random_sample())
-                    Q_table[x] = u
-        return Q_table
-
-    def state_init(self):
-        """
-        initialize a starting state
-        :return: starting state
-        """
-        while True:
-            i = np.random.randint(0, self.env.x_range - 1)
-            j = np.random.randint(0, self.env.y_range - 1)
-            if (i, j) not in self.obs:
-                return (i, j)
-
-    def epsilon_greedy(self, u, error):
-        """
-        generate a policy using epsilon_greedy algorithm
-
-        :param u: original input
-        :param error: epsilon value
-        :return: epsilon policy
-        """
-
-        if np.random.random_sample() < 3 / 4 * error:
-            u_e = u
-            while u_e == u:
-                p = np.random.random_sample()
-                if p < 0.25:
-                    u_e = 0
-                elif p < 0.5:
-                    u_e = 1
-                elif p < 0.75:
-                    u_e = 2
-                else:
-                    u_e = 3
-            return u_e
-        return u
-
-    def move_next(self, x, u):
-        """
-        get next state.
-
-        :param x: current state
-        :param u: input
-        :return: next state
-        """
-
-        x_next = (x[0] + u[0], x[1] + u[1])
-        if x_next in self.obs:
-            return x
-        return x_next
-
-    def extract_path(self, xI, xG, policy):
-        """
-        extract path from converged policy.
-
-        :param xI: starting state
-        :param xG: goal states
-        :param policy: converged policy
-        :return: path
-        """
-
-        x, path = xI, [xI]
-        while x != xG:
-            u = self.u_set[policy[x]]
-            x_next = (x[0] + u[0], x[1] + u[1])
-            if x_next in self.obs:
-                print("Collision! Please run again!")
-                return path
-            else:
-                path.append(x_next)
-                x = x_next
-        return path
-
-    def message(self):
-        """
-        print important message.
-
-        :param count: iteration numbers
-        :return: print
-        """
-
-        print("starting state: ", self.xI)
-        print("goal state: ", self.xG)
-        print("iteration numbers: ", self.M)
-        print("discount factor: ", self.gamma)
-        print("epsilon error: ", self.epsilon)
-        print("alpha: ", self.alpha)
-
-
-if __name__ == '__main__':
-    x_Start = (1, 1)
-    x_Goal = (12, 1)
-
-    SARSA_CALL = SARSA(x_Start, x_Goal)

Model-free Control/env.py

@@ -1,70 +0,0 @@
-class Env:
-    def __init__(self, xI, xG):
-        self.x_range = 14                           # size of background
-        self.y_range = 6
-        self.motions = [(1, 0), (-1, 0), (0, 1), (0, -1)]
-        self.xI = xI
-        self.xG = xG
-        self.obs = self.obs_map()
-        self.lose = self.lose_map()
-        self.stateSpace = self.state_space()
-
-    def obs_map(self):
-        """
-        Initialize obstacles' positions
-
-        :return: map of obstacles
-        """
-        x = self.x_range
-        y = self.y_range
-        obs = []
-
-        for i in range(x):
-            obs.append((i, 0))
-        for i in range(x):
-            obs.append((i, y - 1))
-
-        for i in range(y):
-            obs.append((0, i))
-        for i in range(y):
-            obs.append((x - 1, i))
-
-        return obs
-
-    def lose_map(self):
-        """
-        Initialize losing states' positions
-        :return: losing states
-        """
-
-        lose = []
-        for i in range(2, 12):
-            lose.append((i, 1))
-
-        return lose
-
-    def state_space(self):
-        """
-        generate state space
-        :return: state space
-        """
-
-        state_space = []
-        for i in range(self.x_range):
-            for j in range(self.y_range):
-                if (i, j) not in self.obs:
-                    state_space.append((i, j))
-
-        return state_space
-
-    def get_reward(self, x_next):
-        """
-        calculate reward of next state
-
-        :param x_next: next state
-        :return: reward
-        """
-
-        if x_next in self.lose:
-            return -100                         # reward : -100, for lose states
-        return -1                               # reward : -1, for other states

Model-free Control/motion_model.py

@@ -1,38 +0,0 @@
-import env
-
-
-class Motion_model():
-    def __init__(self, xI, xG):
-        self.env = env.Env(xI, xG)
-        self.obs = self.env.obs_map()
-
-    def move_next(self, x, u, eta=0.2):
-        """
-        Motion model of robots,
-
-        :param x: current state (node)
-        :param u: input
-        :param obs: obstacle map
-        :param eta: noise in motion model
-        :return: next states and corresponding probability
-        """
-
-        p_next = [1 - eta, eta / 2, eta / 2]
-        x_next = []
-        if u == (0, 1):
-            u_real = [(0, 1), (-1, 0), (1, 0)]
-        elif u == (0, -1):
-            u_real = [(0, -1), (-1, 0), (1, 0)]
-        elif u == (-1, 0):
-            u_real = [(-1, 0), (0, 1), (0, -1)]
-        else:
-            u_real = [(1, 0), (0, 1), (0, -1)]
-
-        for act in u_real:
-            x_check = (x[0] + act[0], x[1] + act[1])
-            if x_check in self.obs:
-                x_next.append(x)
-            else:
-                x_next.append(x_check)
-
-        return x_next, p_next

Model-free Control/plotting.py

@@ -1,110 +0,0 @@
-import matplotlib.pyplot as plt
-import env
-
-
-class Plotting():
-    def __init__(self, xI, xG):
-        self.xI, self.xG = xI, xG
-        self.env = env.Env(self.xI, self.xG)
-        self.obs = self.env.obs_map()
-        self.lose = self.env.lose_map()
-
-    def animation(self, path, name):
-        """
-        animation.
-
-        :param path: optimal path
-        :param name: tile of figure
-        :return: an animation
-        """
-
-        plt.figure(1)
-        self.plot_grid(name)
-        self.plot_lose()
-        self.plot_path(path)
-
-    def plot_grid(self, name):
-        """
-        plot the obstacles in environment.
-
-        :param name: title of figure
-        :return: plot
-        """
-
-        obs_x = [self.obs[i][0] for i in range(len(self.obs))]
-        obs_y = [self.obs[i][1] for i in range(len(self.obs))]
-
-        plt.plot(self.xI[0], self.xI[1], "bs", ms=24)
-        plt.plot(self.xG[0], self.xG[1], "gs", ms=24)
-
-        plt.plot(obs_x, obs_y, "sk", ms=24)
-        plt.title(name)
-        plt.axis("equal")
-
-    def plot_lose(self):
-        """
-        plot losing states in environment.
-        :return: a plot
-        """
-
-        lose_x = [self.lose[i][0] for i in range(len(self.lose))]
-        lose_y = [self.lose[i][1] for i in range(len(self.lose))]
-
-        plt.plot(lose_x, lose_y, color='#A52A2A', marker='s', ms=24)
-
-    def plot_visited(self, visited):
-        """
-        animation of order of visited nodes.
-
-        :param visited: visited nodes
-        :return: animation
-        """
-
-        visited.remove(self.xI)
-        count = 0
-
-        for x in visited:
-            count += 1
-            plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o')
-            plt.gcf().canvas.mpl_connect('key_release_event', lambda event:
-            [exit(0) if event.key == 'escape' else None])
-
-            if count < len(visited) / 3:
-                length = 15
-            elif count < len(visited) * 2 / 3:
-                length = 30
-            else:
-                length = 45
-
-            if count % length == 0: plt.pause(0.001)
-
-    def plot_path(self, path):
-        path.remove(self.xI)
-        path.remove(self.xG)
-
-        for x in path:
-            plt.plot(x[0], x[1], color='#808080', marker='o', ms=23)
-            plt.gcf().canvas.mpl_connect('key_release_event', lambda event:
-            [exit(0) if event.key == 'escape' else None])
-            plt.pause(0.001)
-        plt.show()
-        plt.pause(0.5)
-
-    def plot_diff(self, diff, name):
-        plt.figure(2)
-        plt.title(name, fontdict=None)
-        plt.xlabel('iterations')
-        plt.ylabel('difference of successive iterations')
-        plt.grid('on')
-
-        count = 0
-        for x in diff:
-            plt.plot(count, x, color='#808080', marker='o')  # plot dots for animation
-            plt.gcf().canvas.mpl_connect('key_release_event', lambda event:
-            [exit(0) if event.key == 'escape' else None])
-            plt.pause(0.07)
-            count += 1
-
-        plt.plot(diff, color='#808080')
-        plt.pause(0.01)
-        plt.show()

README.md

@@ -1,49 +1,47 @@
+Overview
+------
+This repository implements some common path planning algorithms used in robotics, including Search-based algorithms and Sampling-based algorithms. We designed animation for each algorithm to display the running process.
+
 Directory Structure
 ------
     .
     └── Search-based Planning
-        └── Search_2D
-            ├── bfs.py                                  # breadth-first searching
-            ├── dfs.py                                  # depth-first searching
-            ├── dijkstra.py                             # dijkstra's
-            ├── a_star.py                               # A*
-            ├── bidirectional_a_star.py                 # Bidirectional A*
-            ├── ARAstar.py                              # Anytime Reparing A*
-            ├── IDAstar.py                              # Iteratively Deepening A*
-            ├── LRTAstar.py                             # Learning Real-time A*
-            ├── RTAAstar.py                             # Real-time Adaptive A*
-            ├── LPAstar.py                              # Lifelong Planning A*
-            ├── D_star.py                               # D* (Dynamic A*)
-            ├── Anytime_D_star.py                       # Anytime D*
-            └── D_star_Lite.py                          # D* Lite
-        └── Search_3D
-            ├── Astar3D.py                              # A*_3D
-            ├── bidirectional_Astar3D.py                # Bidirectional A*_3D
-            ├── RTA_Astar3D.py                          # Real-time Adaptive A*_3D
-            └── LRT_Astar3D.py                          # Learning Real-time A*_3D
+        ├── Breadth-First Searching (BFS)
+        ├── Depth-First Searching (DFS)
+        ├── Best-First Searching
+        ├── Dijkstra's
+        ├── A*
+        ├── Bidirectional A*
+        ├── Anytime Repairing A*
+        ├── Learning Real-time A* (LRTA*)
+        ├── Real-time Adaptive A* (RTAA*)
+        ├── Lifelong Planning A* (LPA*)
+        ├── Dynamic A* (D*)
+        ├── D* Lite
+        ├── Anytime D*
+        └── Potential Field
     └── Sampling-based Planning
-        └── rrt_2D
-            ├── rrt.py                                  # rrt : goal-biased rrt
-            └── rrt_star.py
-        └── rrt_3D
-            ├── rrt3D.py                                # rrt3D : goal-biased rrt3D
-            └── rrtstar3D.py
-    └── Stochastic Shortest Path
-        ├── value_iteration.py                          # value iteration
-        ├── policy_iteration.py                         # policy iteration
-        ├── Q-value_iteration.py                        # Q-value iteration
-        └── Q-policy_iteration.py                       # Q-policy iteration
-    └── Model-free Control
-        ├── Sarsa.py                                    # SARSA : on-policy TD control
-        └── Q-learning.py                               # Q-learning : off-policy TD control
+        ├── RRT
+        ├── RRT-Connect
+        ├── Extended-RRT
+        ├── Dynamic-RRT
+        ├── RRT*
+        ├── Informed RRT*
+        ├── RRT* Smart
+        ├── Anytime RRT*
+        ├── Closed-Loop RRT*
+        ├── Spline-RRT*
+        ├── LQR-RRT*
+        ├── Fast Marching Trees (FMT*)
+        └── Batch Informed Trees (BIT*)
 
 ## Animations - Search-Based
 ### Best-First & Dijkstra
 <div align=right>
 <table>
   <tr>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/BF.gif" alt="dfs" width="400"/></a></td>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/Dijkstra.gif" alt="dijkstra" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/BF.gif" alt="dfs" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/Dijkstra.gif" alt="dijkstra" width="400"/></a></td>
   </tr>
 </table>
 </div>
@@ -52,32 +50,32 @@ Directory Structure
 <div align=right>
 <table>
   <tr>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/Astar.gif" alt="astar" width="400"/></a></td>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/Bi-Astar.gif" alt="biastar" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/Astar.gif" alt="astar" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/Bi-Astar.gif" alt="biastar" width="400"/></a></td>
   </tr>
 </table>
 <table>
   <tr>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/RepeatedA_star.gif" alt="repeatedastar" width="400"/></a></td>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/ARA_star.gif" alt="arastar" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/RepeatedA_star.gif" alt="repeatedastar" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/ARA_star.gif" alt="arastar" width="400"/></a></td>
   </tr>
 </table>
 <table>
   <tr>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/LRTA_star.gif" alt="lrtastar" width="400"/></a></td>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/RTAA_star.gif" alt="rtaastar" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/LRTA_star.gif" alt="lrtastar" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/RTAA_star.gif" alt="rtaastar" width="400"/></a></td>
   </tr>
 </table>
 <table>
   <tr>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/LPAstar.gif" alt="lpastar" width="400"/></a></td>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/D_star_Lite.gif" alt="dstarlite" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/LPAstar.gif" alt="lpastar" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/D_star_Lite.gif" alt="dstarlite" width="400"/></a></td>
   </tr>
 </table>
 <table>
   <tr>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/ADstar_small.gif" alt="lpastar" width="400"/></a></td>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/ADstar_sig.gif" alt="dstarlite" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/ADstar_small.gif" alt="lpastar" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/ADstar_sig.gif" alt="dstarlite" width="400"/></a></td>
   </tr>
 </table>
 </div>
@@ -87,45 +85,44 @@ Directory Structure
 <div align=right>
 <table>
   <tr>
-    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling-based%20Planning/gif/Goal_biasd_RRT_2D.gif" alt="value iteration" width="400"/></a></td>
-    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling-based%20Planning/gif/RRT_CONNECT_2D.gif" alt="value iteration" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/RRT_2D.gif" alt="value iteration" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/Goal_biasd_RRT_2D.gif" alt="value iteration" width="400"/></a></td>
   </tr>
 </table>
 <table>
   <tr>
-    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling-based%20Planning/gif/Extended_RRT_2D.gif" alt="value iteration" width="400"/></a></td>
-    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling-based%20Planning/gif/Dynamic_RRT_2D.gif" alt="value iteration" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/RRT_CONNECT_2D.gif" alt="value iteration" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/Extended_RRT_2D.gif" alt="value iteration" width="400"/></a></td>
   </tr>
 </table>
-</div>
-
-### Value/Policy/Q-value/Q-policy Iteration
-* Brown: losing states
-<div align=right>
 <table>
   <tr>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Stochastic%20Shortest%20Path/gif/VI.gif" alt="value iteration" width="400"/></a></td>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Stochastic%20Shortest%20Path/gif/VI_E.gif" alt="value iteration" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/Dynamic_RRT_2D.gif" alt="value iteration" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/RRT_STAR2_2D.gif" alt="value iteration" width="400"/></a></td>
   </tr>
 </table>
-</div>
-
-### SARSA(on-policy) & Q-learning(off-policy)
-* Brown: losing states
-<div align=right>
 <table>
   <tr>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Model-free%20Control/gif/SARSA.gif" alt="value iteration" width="400"/></a></td>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Model-free%20Control/gif/Qlearning.gif" alt="value iteration" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/RRT_STAR_SMART_2D.gif" alt="value iteration" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/FMT.gif" alt="value iteration" width="400"/></a></td>
+  </tr>
+</table>
+<table>
+  <tr>
+    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D3.gif" alt="value iteration" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/BIT2.gif" alt="value iteration" width="400"/></a></td>
   </tr>
 </table>
 </div>
 
 ## Papers
 ### Search-base Planning
-* [D*: ](http://web.mit.edu/16.412j/www/html/papers/original_dstar_icra94.pdf) Optimal and Efficient Path Planning for Partially-Known Environments
+* [A*: ](https://ieeexplore.ieee.org/document/4082128) A Formal Basis for the Heuristic Determination of Minimum Cost Paths
+* [Learning Real-Time A*: ](https://arxiv.org/pdf/1110.4076.pdf) Learning in Real-Time Search: A Unifying Framework
+* [Real-Time Adaptive A*: ](http://idm-lab.org/bib/abstracts/papers/aamas06.pdf) Real-Time Adaptive A*
 * [Lifelong Planning A*: ](https://www.cs.cmu.edu/~maxim/files/aij04.pdf) Lifelong Planning A*
 * [Anytime Repairing A*: ](https://papers.nips.cc/paper/2382-ara-anytime-a-with-provable-bounds-on-sub-optimality.pdf) ARA*: Anytime A* with Provable Bounds on Sub-Optimality
+* [D*: ](http://web.mit.edu/16.412j/www/html/papers/original_dstar_icra94.pdf) Optimal and Efficient Path Planning for Partially-Known Environments
 * [D* Lite: ](http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf) D* Lite
 * [Field D*: ](http://robots.stanford.edu/isrr-papers/draft/stentz.pdf) Field D*: An Interpolation-based Path Planner and Replanner
 * [Anytime D*: ](http://www.cs.cmu.edu/~ggordon/likhachev-etal.anytime-dstar.pdf) Anytime Dynamic A*: An Anytime, Replanning Algorithm
@@ -139,9 +136,15 @@ Directory Structure
 * [Extended-RRT: ](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.1.7617&rep=rep1&type=pdf) Real-Time Randomized Path Planning for Robot Navigation
 * [Dynamic-RRT: ](https://www.ri.cmu.edu/pub_files/pub4/ferguson_david_2006_2/ferguson_david_2006_2.pdf) Replanning with RRTs
 * [RRT*: ](https://journals.sagepub.com/doi/abs/10.1177/0278364911406761) Sampling-based algorithms for optimal motion planning
-* [Bidirectional-RRT*: ](https://dspace.mit.edu/bitstream/handle/1721.1/79884/MIT-CSAIL-TR-2013-021.pdf) Optimal Bidirectional Rapidly-Exploring Random Trees
-* [RRT*-Smart: ](http://save.seecs.nust.edu.pk/pubs/ICMA2012.pdf) Rapid convergence implementation of RRT* towards optimal solution
-* [Anytime-RRT: ](https://dspace.mit.edu/handle/1721.1/63170) Anytime Motion Planning using the RRT*
-* [Closed-loop RRT (CL-RRT): ](http://acl.mit.edu/papers/KuwataTCST09.pdf) Real-time Motion Planning with Applications to Autonomous Urban Driving
+* [Anytime-RRT*: ](https://dspace.mit.edu/handle/1721.1/63170) Anytime Motion Planning using the RRT*
+* [Closed-loop RRT* (CL-RRT*): ](http://acl.mit.edu/papers/KuwataTCST09.pdf) Real-time Motion Planning with Applications to Autonomous Urban Driving
 * [Spline-RRT*: ](https://ieeexplore.ieee.org/abstract/document/6987895?casa_token=B9GUwVDbbncAAAAA:DWscGFLIa97ptgH7NpUQUL0A2ModiiBDBGklk1z7aDjI11Kyfzo8rpuFstdYcjOofJfCjR-mNw) Optimal path planning based on spline-RRT* for fixed-wing UAVs operating in three-dimensional environments
 * [LQR-RRT*: ](https://lis.csail.mit.edu/pubs/perez-icra12.pdf) Optimal Sampling-Based Motion Planning with Automatically Derived Extension Heuristics
+* [RRT#: ](http://dcsl.gatech.edu/papers/icra13.pdf) Use of Relaxation Methods in Sampling-Based Algorithms for Optimal Motion Planning
+* [RRT*-Smart: ](http://save.seecs.nust.edu.pk/pubs/ICMA2012.pdf) Rapid convergence implementation of RRT* towards optimal solution
+* [Informed RRT*: ](https://arxiv.org/abs/1404.2334) Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic
+* [Fast Marching Trees (FMT*): ](https://arxiv.org/abs/1306.3532) a Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions
+* [Motion Planning using Lower Bounds (MPLB): ](https://ieeexplore.ieee.org/document/7139773) Asymptotically-optimal Motion Planning using lower bounds on cost
+* [Batch Informed Trees (BIT*): ](https://arxiv.org/abs/1405.5848) Sampling-based Optimal Planning via the Heuristically Guided Search of Implicit Random Geometric Graphs
+* [Advanced Batch Informed Trees (ABIT*): ](https://arxiv.org/abs/2002.06589) Sampling-Based Planning with Advanced Graph-Search Techniques ((ICRA) 2020)
+* [Adaptively Informed Trees (AIT*): ](https://arxiv.org/abs/2002.06599) Fast Asymptotically Optimal Path Planning through Adaptive Heuristics ((ICRA) 2020)

Sampling-based Planning/.idea/.gitignore

@@ -1,3 +0,0 @@
-# Default ignored files
-/shelf/
-/workspace.xml

Sampling-based Planning/.idea/Sampling-based Planning.iml

@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<module type="PYTHON_MODULE" version="4">
-  <component name="NewModuleRootManager">
-    <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="Python 3.7" jdkType="Python SDK" />
-    <orderEntry type="sourceFolder" forTests="false" />
-  </component>
-</module>

Sampling-based Planning/.idea/dictionaries/zhou.xml

@@ -1,8 +0,0 @@
-<component name="ProjectDictionaryState">
-  <dictionary name="zhou">
-    <words>
-      <w>huiming</w>
-      <w>zhou</w>
-    </words>
-  </dictionary>
-</component>

Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml

@@ -1,6 +0,0 @@
-<component name="InspectionProjectProfileManager">
-  <settings>
-    <option name="USE_PROJECT_PROFILE" value="false" />
-    <version value="1.0" />
-  </settings>
-</component>

Sampling-based Planning/.idea/misc.xml

@@ -1,4 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
-</project>

Sampling-based Planning/.idea/modules.xml

@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectModuleManager">
-    <modules>
-      <module fileurl="file://$PROJECT_DIR$/.idea/Sampling-based Planning.iml" filepath="$PROJECT_DIR$/.idea/Sampling-based Planning.iml" />
-    </modules>
-  </component>
-</project>

Sampling-based Planning/.idea/vcs.xml

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="VcsDirectoryMappings">
-    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
-  </component>
-</project>

Sampling_based_Planning/rrt_2D/batch_informed_trees.py

@@ -0,0 +1,402 @@
+"""
+Batch Informed Trees (BIT*)
+@author: huiming zhou
+"""
+
+import os
+import sys
+import math
+import random
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+from scipy.spatial.transform import Rotation as Rot
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Sampling_based_Planning/")
+
+from Sampling_based_Planning.rrt_2D import env, plotting, utils
+
+
+class Node:
+    def __init__(self, x, y):
+        self.x = x
+        self.y = y
+        self.parent = None
+
+
+class Tree:
+    def __init__(self, x_start, x_goal):
+        self.x_start = x_start
+        self.goal = x_goal
+
+        self.r = 4.0
+        self.V = set()
+        self.E = set()
+        self.QE = set()
+        self.QV = set()
+
+        self.V_old = set()
+
+
+class BITStar:
+    def __init__(self, x_start, x_goal, eta, iter_max):
+        self.x_start = Node(x_start[0], x_start[1])
+        self.x_goal = Node(x_goal[0], x_goal[1])
+        self.eta = eta
+        self.iter_max = iter_max
+
+        self.env = env.Env()
+        self.plotting = plotting.Plotting(x_start, x_goal)
+        self.utils = utils.Utils()
+
+        self.fig, self.ax = plt.subplots()
+
+        self.delta = self.utils.delta
+        self.x_range = self.env.x_range
+        self.y_range = self.env.y_range
+
+        self.obs_circle = self.env.obs_circle
+        self.obs_rectangle = self.env.obs_rectangle
+        self.obs_boundary = self.env.obs_boundary
+
+        self.Tree = Tree(self.x_start, self.x_goal)
+        self.X_sample = set()
+        self.g_T = dict()
+
+    def init(self):
+        self.Tree.V.add(self.x_start)
+        self.X_sample.add(self.x_goal)
+
+        self.g_T[self.x_start] = 0.0
+        self.g_T[self.x_goal] = np.inf
+
+        cMin, theta = self.calc_dist_and_angle(self.x_start, self.x_goal)
+        C = self.RotationToWorldFrame(self.x_start, self.x_goal, cMin)
+        xCenter = np.array([[(self.x_start.x + self.x_goal.x) / 2.0],
+                            [(self.x_start.y + self.x_goal.y) / 2.0], [0.0]])
+
+        return theta, cMin, xCenter, C
+
+    def planning(self):
+        theta, cMin, xCenter, C = self.init()
+
+        for k in range(500):
+            if not self.Tree.QE and not self.Tree.QV:
+                if k == 0:
+                    m = 350
+                else:
+                    m = 200
+
+                if self.x_goal.parent is not None:
+                    path_x, path_y = self.ExtractPath()
+                    plt.plot(path_x, path_y, linewidth=2, color='r')
+                    plt.pause(0.5)
+
+                self.Prune(self.g_T[self.x_goal])
+                self.X_sample.update(self.Sample(m, self.g_T[self.x_goal], cMin, xCenter, C))
+                self.Tree.V_old = {v for v in self.Tree.V}
+                self.Tree.QV = {v for v in self.Tree.V}
+                # self.Tree.r = self.radius(len(self.Tree.V) + len(self.X_sample))
+
+            while self.BestVertexQueueValue() <= self.BestEdgeQueueValue():
+                self.ExpandVertex(self.BestInVertexQueue())
+
+            vm, xm = self.BestInEdgeQueue()
+            self.Tree.QE.remove((vm, xm))
+
+            if self.g_T[vm] + self.calc_dist(vm, xm) + self.h_estimated(xm) < self.g_T[self.x_goal]:
+                actual_cost = self.cost(vm, xm)
+                if self.g_estimated(vm) + actual_cost + self.h_estimated(xm) < self.g_T[self.x_goal]:
+                    if self.g_T[vm] + actual_cost < self.g_T[xm]:
+                        if xm in self.Tree.V:
+                            # remove edges
+                            edge_delete = set()
+                            for v, x in self.Tree.E:
+                                if x == xm:
+                                    edge_delete.add((v, x))
+
+                            for edge in edge_delete:
+                                self.Tree.E.remove(edge)
+                        else:
+                            self.X_sample.remove(xm)
+                            self.Tree.V.add(xm)
+                            self.Tree.QV.add(xm)
+
+                        self.g_T[xm] = self.g_T[vm] + actual_cost
+                        self.Tree.E.add((vm, xm))
+                        xm.parent = vm
+
+                        set_delete = set()
+                        for v, x in self.Tree.QE:
+                            if x == xm and self.g_T[v] + self.calc_dist(v, xm) >= self.g_T[xm]:
+                                set_delete.add((v, x))
+
+                        for edge in set_delete:
+                            self.Tree.QE.remove(edge)
+            else:
+                self.Tree.QE = set()
+                self.Tree.QV = set()
+
+            if k % 5 == 0:
+                self.animation(xCenter, self.g_T[self.x_goal], cMin, theta)
+
+        path_x, path_y = self.ExtractPath()
+        plt.plot(path_x, path_y, linewidth=2, color='r')
+        plt.pause(0.01)
+        plt.show()
+
+    def ExtractPath(self):
+        node = self.x_goal
+        path_x, path_y = [node.x], [node.y]
+
+        while node.parent:
+            node = node.parent
+            path_x.append(node.x)
+            path_y.append(node.y)
+
+        return path_x, path_y
+
+    def Prune(self, cBest):
+        self.X_sample = {x for x in self.X_sample if self.f_estimated(x) < cBest}
+        self.Tree.V = {v for v in self.Tree.V if self.f_estimated(v) <= cBest}
+        self.Tree.E = {(v, w) for v, w in self.Tree.E
+                       if self.f_estimated(v) <= cBest and self.f_estimated(w) <= cBest}
+        self.X_sample.update({v for v in self.Tree.V if self.g_T[v] == np.inf})
+        self.Tree.V = {v for v in self.Tree.V if self.g_T[v] < np.inf}
+
+    def cost(self, start, end):
+        if self.utils.is_collision(start, end):
+            return np.inf
+
+        return self.calc_dist(start, end)
+
+    def f_estimated(self, node):
+        return self.g_estimated(node) + self.h_estimated(node)
+
+    def g_estimated(self, node):
+        return self.calc_dist(self.x_start, node)
+
+    def h_estimated(self, node):
+        return self.calc_dist(node, self.x_goal)
+
+    def Sample(self, m, cMax, cMin, xCenter, C):
+        if cMax < np.inf:
+            return self.SampleEllipsoid(m, cMax, cMin, xCenter, C)
+        else:
+            return self.SampleFreeSpace(m)
+
+    def SampleEllipsoid(self, m, cMax, cMin, xCenter, C):
+        r = [cMax / 2.0,
+             math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,
+             math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]
+        L = np.diag(r)
+
+        ind = 0
+        delta = self.delta
+        Sample = set()
+
+        while ind < m:
+            xBall = self.SampleUnitNBall()
+            x_rand = np.dot(np.dot(C, L), xBall) + xCenter
+            node = Node(x_rand[(0, 0)], x_rand[(1, 0)])
+            in_obs = self.utils.is_inside_obs(node)
+            in_x_range = self.x_range[0] + delta <= node.x <= self.x_range[1] - delta
+            in_y_range = self.y_range[0] + delta <= node.y <= self.y_range[1] - delta
+
+            if not in_obs and in_x_range and in_y_range:
+                Sample.add(node)
+                ind += 1
+
+        return Sample
+
+    def SampleFreeSpace(self, m):
+        delta = self.utils.delta
+        Sample = set()
+
+        ind = 0
+        while ind < m:
+            node = Node(random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+                        random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))
+            if self.utils.is_inside_obs(node):
+                continue
+            else:
+                Sample.add(node)
+                ind += 1
+
+        return Sample
+
+    def radius(self, q):
+        cBest = self.g_T[self.x_goal]
+        lambda_X = len([1 for v in self.Tree.V if self.f_estimated(v) <= cBest])
+        radius = 2 * self.eta * (1.5 * lambda_X / math.pi * math.log(q) / q) ** 0.5
+
+        return radius
+
+    def ExpandVertex(self, v):
+        self.Tree.QV.remove(v)
+        X_near = {x for x in self.X_sample if self.calc_dist(x, v) <= self.Tree.r}
+
+        for x in X_near:
+            if self.g_estimated(v) + self.calc_dist(v, x) + self.h_estimated(x) < self.g_T[self.x_goal]:
+                self.g_T[x] = np.inf
+                self.Tree.QE.add((v, x))
+
+        if v not in self.Tree.V_old:
+            V_near = {w for w in self.Tree.V if self.calc_dist(w, v) <= self.Tree.r}
+
+            for w in V_near:
+                if (v, w) not in self.Tree.E and \
+                        self.g_estimated(v) + self.calc_dist(v, w) + self.h_estimated(w) < self.g_T[self.x_goal] and \
+                        self.g_T[v] + self.calc_dist(v, w) < self.g_T[w]:
+                    self.Tree.QE.add((v, w))
+                    if w not in self.g_T:
+                        self.g_T[w] = np.inf
+
+    def BestVertexQueueValue(self):
+        if not self.Tree.QV:
+            return np.inf
+
+        return min(self.g_T[v] + self.h_estimated(v) for v in self.Tree.QV)
+
+    def BestEdgeQueueValue(self):
+        if not self.Tree.QE:
+            return np.inf
+
+        return min(self.g_T[v] + self.calc_dist(v, x) + self.h_estimated(x)
+                   for v, x in self.Tree.QE)
+
+    def BestInVertexQueue(self):
+        if not self.Tree.QV:
+            print("QV is Empty!")
+            return None
+
+        v_value = {v: self.g_T[v] + self.h_estimated(v) for v in self.Tree.QV}
+
+        return min(v_value, key=v_value.get)
+
+    def BestInEdgeQueue(self):
+        if not self.Tree.QE:
+            print("QE is Empty!")
+            return None
+
+        e_value = {(v, x): self.g_T[v] + self.calc_dist(v, x) + self.h_estimated(x)
+                   for v, x in self.Tree.QE}
+
+        return min(e_value, key=e_value.get)
+
+    @staticmethod
+    def SampleUnitNBall():
+        while True:
+            x, y = random.uniform(-1, 1), random.uniform(-1, 1)
+            if x ** 2 + y ** 2 < 1:
+                return np.array([[x], [y], [0.0]])
+
+    @staticmethod
+    def RotationToWorldFrame(x_start, x_goal, L):
+        a1 = np.array([[(x_goal.x - x_start.x) / L],
+                       [(x_goal.y - x_start.y) / L], [0.0]])
+        e1 = np.array([[1.0], [0.0], [0.0]])
+        M = a1 @ e1.T
+        U, _, V_T = np.linalg.svd(M, True, True)
+        C = U @ np.diag([1.0, 1.0, np.linalg.det(U) * np.linalg.det(V_T.T)]) @ V_T
+
+        return C
+
+    @staticmethod
+    def calc_dist(start, end):
+        return math.hypot(start.x - end.x, start.y - end.y)
+
+    @staticmethod
+    def calc_dist_and_angle(node_start, node_end):
+        dx = node_end.x - node_start.x
+        dy = node_end.y - node_start.y
+        return math.hypot(dx, dy), math.atan2(dy, dx)
+
+    def animation(self, xCenter, cMax, cMin, theta):
+        plt.cla()
+        self.plot_grid("Batch Informed Trees (BIT*)")
+
+        plt.gcf().canvas.mpl_connect(
+            'key_release_event',
+            lambda event: [exit(0) if event.key == 'escape' else None])
+
+        for v in self.X_sample:
+            plt.plot(v.x, v.y, marker='.', color='lightgrey', markersize='2')
+
+        if cMax < np.inf:
+            self.draw_ellipse(xCenter, cMax, cMin, theta)
+
+        for v, w in self.Tree.E:
+            plt.plot([v.x, w.x], [v.y, w.y], '-g')
+
+        plt.pause(0.001)
+
+    def plot_grid(self, name):
+        for (ox, oy, w, h) in self.obs_boundary:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='black',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, w, h) in self.obs_rectangle:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, r) in self.obs_circle:
+            self.ax.add_patch(
+                patches.Circle(
+                    (ox, oy), r,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        plt.plot(self.x_start.x, self.x_start.y, "bs", linewidth=3)
+        plt.plot(self.x_goal.x, self.x_goal.y, "rs", linewidth=3)
+
+        plt.title(name)
+        plt.axis("equal")
+
+    @staticmethod
+    def draw_ellipse(x_center, c_best, dist, theta):
+        a = math.sqrt(c_best ** 2 - dist ** 2) / 2.0
+        b = c_best / 2.0
+        angle = math.pi / 2.0 - theta
+        cx = x_center[0]
+        cy = x_center[1]
+        t = np.arange(0, 2 * math.pi + 0.1, 0.2)
+        x = [a * math.cos(it) for it in t]
+        y = [b * math.sin(it) for it in t]
+        rot = Rot.from_euler('z', -angle).as_dcm()[0:2, 0:2]
+        fx = rot @ np.array([x, y])
+        px = np.array(fx[0, :] + cx).flatten()
+        py = np.array(fx[1, :] + cy).flatten()
+        plt.plot(cx, cy, marker='.', color='darkorange')
+        plt.plot(px, py, linestyle='--', color='darkorange', linewidth=2)
+
+
+def main():
+    x_start = (18, 8)  # Starting node
+    x_goal = (37, 18)  # Goal node
+    eta = 2
+    iter_max = 200
+    print("start!!!")
+    bit = BITStar(x_start, x_goal, eta, iter_max)
+    # bit.animation("Batch Informed Trees (BIT*)")
+    bit.planning()
+
+
+if __name__ == '__main__':
+    main()

Sampling_based_Planning/rrt_2D/dubins_rrt_star.py

@@ -0,0 +1,321 @@
+"""
+DUBINS_RRT_STAR 2D
+@author: huiming zhou
+"""
+
+import os
+import sys
+import math
+import random
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+from scipy.spatial.transform import Rotation as Rot
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Sampling_based_Planning/")
+
+from Sampling_based_Planning.rrt_2D import env, plotting, utils
+import CurvesGenerator.dubins_path as dubins
+import CurvesGenerator.draw as draw
+
+
+class Node:
+    def __init__(self, x, y, yaw):
+        self.x = x
+        self.y = y
+        self.yaw = yaw
+        self.parent = None
+        self.cost = 0.0
+        self.path_x = []
+        self.path_y = []
+        self.paty_yaw = []
+
+
+class DubinsRRTStar:
+    def __init__(self, sx, sy, syaw, gx, gy, gyaw, vehicle_radius, step_len,
+                 goal_sample_rate, search_radius, iter_max):
+        self.s_start = Node(sx, sy, syaw)
+        self.s_goal = Node(gx, gy, gyaw)
+        self.vr = vehicle_radius
+        self.step_len = step_len
+        self.goal_sample_rate = goal_sample_rate
+        self.search_radius = search_radius
+        self.iter_max = iter_max
+        self.curv = 1
+
+        self.env = env.Env()
+        self.utils = utils.Utils()
+
+        self.fig, self.ax = plt.subplots()
+        self.delta = self.utils.delta
+        self.x_range = self.env.x_range
+        self.y_range = self.env.y_range
+        self.obs_circle = self.obs_circle()
+        self.obs_boundary = self.env.obs_boundary
+        self.utils.update_obs(self.obs_circle, self.obs_boundary, [])
+
+        self.V = [self.s_start]
+        self.path = None
+
+    def planning(self):
+
+        for i in range(self.iter_max):
+            print("Iter:", i, ", number of nodes:", len(self.V))
+            rnd = self.Sample()
+            node_nearest = self.Nearest(self.V, rnd)
+            new_node = self.Steer(node_nearest, rnd)
+
+            if new_node and not self.is_collision(new_node):
+                near_indexes = self.Near(self.V, new_node)
+                new_node = self.choose_parent(new_node, near_indexes)
+
+                if new_node:
+                    self.V.append(new_node)
+                    self.rewire(new_node, near_indexes)
+
+            if i % 5 == 0:
+                self.draw_graph()
+
+        last_index = self.search_best_goal_node()
+
+        path = self.generate_final_course(last_index)
+        print("get!")
+        px = [s[0] for s in path]
+        py = [s[1] for s in path]
+        plt.plot(px, py, '-r')
+        plt.pause(0.01)
+        plt.show()
+
+    def draw_graph(self, rnd=None):
+        plt.cla()
+        # for stopping simulation with the esc key.
+        plt.gcf().canvas.mpl_connect('key_release_event',
+                                     lambda event: [exit(0) if event.key == 'escape' else None])
+        for node in self.V:
+            if node.parent:
+                plt.plot(node.path_x, node.path_y, "-g")
+
+        self.plot_grid("dubins rrt*")
+        plt.plot(self.s_start.x, self.s_start.y, "xr")
+        plt.plot(self.s_goal.x, self.s_goal.y, "xr")
+        plt.grid(True)
+        self.plot_start_goal_arrow()
+        plt.pause(0.01)
+
+    def plot_start_goal_arrow(self):
+        draw.Arrow(self.s_start.x, self.s_start.y, self.s_start.yaw, 2, "darkorange")
+        draw.Arrow(self.s_goal.x, self.s_goal.y, self.s_goal.yaw, 2, "darkorange")
+
+    def generate_final_course(self, goal_index):
+        print("final")
+        path = [[self.s_goal.x, self.s_goal.y]]
+        node = self.V[goal_index]
+        while node.parent:
+            for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
+                path.append([ix, iy])
+            node = node.parent
+        path.append([self.s_start.x, self.s_start.y])
+        return path
+
+    def calc_dist_to_goal(self, x, y):
+        dx = x - self.s_goal.x
+        dy = y - self.s_goal.y
+        return math.hypot(dx, dy)
+
+    def search_best_goal_node(self):
+        dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.V]
+        goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.step_len]
+
+        safe_goal_inds = []
+        for goal_ind in goal_inds:
+            t_node = self.Steer(self.V[goal_ind], self.s_goal)
+            if t_node and not self.is_collision(t_node):
+                safe_goal_inds.append(goal_ind)
+
+        if not safe_goal_inds:
+            return None
+
+        min_cost = min([self.V[i].cost for i in safe_goal_inds])
+        for i in safe_goal_inds:
+            if self.V[i].cost == min_cost:
+                return i
+
+        return None
+
+    def rewire(self, new_node, near_inds):
+        for i in near_inds:
+            near_node = self.V[i]
+            edge_node = self.Steer(new_node, near_node)
+            if not edge_node:
+                continue
+            edge_node.cost = self.calc_new_cost(new_node, near_node)
+
+            no_collision = ~self.is_collision(edge_node)
+            improved_cost = near_node.cost > edge_node.cost
+
+            if no_collision and improved_cost:
+                self.V[i] = edge_node
+                self.propagate_cost_to_leaves(new_node)
+
+    def choose_parent(self, new_node, near_inds):
+        if not near_inds:
+            return None
+
+        costs = []
+        for i in near_inds:
+            near_node = self.V[i]
+            t_node = self.Steer(near_node, new_node)
+            if t_node and not self.is_collision(t_node):
+                costs.append(self.calc_new_cost(near_node, new_node))
+            else:
+                costs.append(float("inf"))  # the cost of collision node
+        min_cost = min(costs)
+
+        if min_cost == float("inf"):
+            print("There is no good path.(min_cost is inf)")
+            return None
+
+        min_ind = near_inds[costs.index(min_cost)]
+        new_node = self.Steer(self.V[min_ind], new_node)
+
+        return new_node
+
+    def calc_new_cost(self, from_node, to_node):
+        d, _ = self.get_distance_and_angle(from_node, to_node)
+        return from_node.cost + d
+
+    def propagate_cost_to_leaves(self, parent_node):
+        for node in self.V:
+            if node.parent == parent_node:
+                node.cost = self.calc_new_cost(parent_node, node)
+                self.propagate_cost_to_leaves(node)
+
+    @staticmethod
+    def get_distance_and_angle(node_start, node_end):
+        dx = node_end.x - node_start.x
+        dy = node_end.y - node_start.y
+        return math.hypot(dx, dy), math.atan2(dy, dx)
+
+    def Near(self, nodelist, node):
+        n = len(nodelist) + 1
+        r = min(self.search_radius * math.sqrt((math.log(n)) / n), self.step_len)
+
+        dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist]
+        node_near_ind = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2]
+
+        return node_near_ind
+
+    def Steer(self, node_start, node_end):
+        sx, sy, syaw = node_start.x, node_start.y, node_start.yaw
+        gx, gy, gyaw = node_end.x, node_end.y, node_end.yaw
+        maxc = self.curv
+
+        path = dubins.calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, maxc)
+
+        if len(path.x) <= 1:
+            return None
+
+        node_new = Node(path.x[-1], path.y[-1], path.yaw[-1])
+        node_new.path_x = path.x
+        node_new.path_y = path.y
+        node_new.path_yaw = path.yaw
+        node_new.cost = node_start.cost + path.L
+        node_new.parent = node_start
+
+        return node_new
+
+    def Sample(self):
+        delta = self.utils.delta
+
+        if random.random() > self.goal_sample_rate:
+            return Node(random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+                        random.uniform(self.y_range[0] + delta, self.y_range[1] - delta),
+                        random.uniform(-math.pi, math.pi))
+        else:
+            return self.s_goal
+
+    @staticmethod
+    def Nearest(nodelist, n):
+        return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2
+                                       for nd in nodelist]))]
+
+    def is_collision(self, node):
+        for ox, oy, r in self.obs_circle:
+            dx = [ox - x for x in node.path_x]
+            dy = [oy - y for y in node.path_y]
+            dist = np.hypot(dx, dy)
+
+            if min(dist) < r + self.delta:
+                return True
+
+        return False
+
+    def animation(self):
+        self.plot_grid("dubins rrt*")
+        self.plot_arrow()
+        plt.show()
+
+    def plot_arrow(self):
+        draw.Arrow(self.s_start.x, self.s_start.y, self.s_start.yaw, 2.5, "darkorange")
+        draw.Arrow(self.s_goal.x, self.s_goal.y, self.s_goal.yaw, 2.5, "darkorange")
+
+    def plot_grid(self, name):
+
+        for (ox, oy, w, h) in self.obs_boundary:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='black',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, r) in self.obs_circle:
+            self.ax.add_patch(
+                patches.Circle(
+                    (ox, oy), r,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        plt.plot(self.s_start.x, self.s_start.y, "bs", linewidth=3)
+        plt.plot(self.s_goal.x, self.s_goal.y, "gs", linewidth=3)
+
+        plt.title(name)
+        plt.axis("equal")
+
+    @staticmethod
+    def obs_circle():
+        obs_cir = [
+            [10, 10, 3],
+            [15, 22, 3],
+            [22, 8, 2.5],
+            [26, 16, 2],
+            [37, 10, 3],
+            [37, 23, 3],
+            [45, 15, 2]
+        ]
+
+        return obs_cir
+
+
+def main():
+    sx, sy, syaw = 5, 5, np.deg2rad(90)
+    gx, gy, gyaw = 45, 25, np.deg2rad(0)
+    goal_sample_rate = 0.1
+    search_radius = 50.0
+    step_len = 30.0
+    iter_max = 250
+    vehicle_radius = 2.0
+
+    drrtstar = DubinsRRTStar(sx, sy, syaw, gx, gy, gyaw, vehicle_radius, step_len,
+                             goal_sample_rate, search_radius, iter_max)
+    drrtstar.planning()
+
+
+if __name__ == '__main__':
+    main()

Sampling-based Planning/rrt_2D/dynamic_rrt.py → Sampling_based_Planning/rrt_2D/dynamic_rrt.py

@@ -12,11 +12,9 @@ import matplotlib.pyplot as plt
 import matplotlib.patches as patches
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
-                "/../../Sampling-based Planning/")
+                "/../../Sampling_based_Planning/")
 
-from rrt_2D import env
-from rrt_2D import plotting
-from rrt_2D import utils
+from Sampling_based_Planning.rrt_2D import env, plotting, utils
 
 
 class Node:

Sampling-based Planning/rrt_2D/extended_rrt.py → Sampling_based_Planning/rrt_2D/extended_rrt.py

@@ -11,11 +11,9 @@ import matplotlib.pyplot as plt
 import matplotlib.patches as patches
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
-                "/../../Sampling-based Planning/")
+                "/../../Sampling_based_Planning/")
 
-from rrt_2D import env
-from rrt_2D import plotting
-from rrt_2D import utils
+from Sampling_based_Planning.rrt_2D import env, plotting, utils
 
 
 class Node:

Sampling_based_Planning/rrt_2D/fast_marching_trees.py

@@ -0,0 +1,220 @@
+"""
+Fast Marching Trees (FMT*)
+@author: huiming zhou
+"""
+
+import os
+import sys
+import math
+import random
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Sampling_based_Planning/")
+
+from Sampling_based_Planning.rrt_2D import env, plotting, utils
+
+
+class Node:
+    def __init__(self, n):
+        self.x = n[0]
+        self.y = n[1]
+        self.parent = None
+        self.cost = np.inf
+
+
+class FMT:
+    def __init__(self, x_start, x_goal, search_radius):
+        self.x_init = Node(x_start)
+        self.x_goal = Node(x_goal)
+        self.search_radius = search_radius
+
+        self.env = env.Env()
+        self.plotting = plotting.Plotting(x_start, x_goal)
+        self.utils = utils.Utils()
+
+        self.fig, self.ax = plt.subplots()
+        self.delta = self.utils.delta
+        self.x_range = self.env.x_range
+        self.y_range = self.env.y_range
+        self.obs_circle = self.env.obs_circle
+        self.obs_rectangle = self.env.obs_rectangle
+        self.obs_boundary = self.env.obs_boundary
+
+        self.V = set()
+        self.V_unvisited = set()
+        self.V_open = set()
+        self.V_closed = set()
+        self.sample_numbers = 1000
+
+    def Init(self):
+        samples = self.SampleFree()
+
+        self.x_init.cost = 0.0
+        self.V.add(self.x_init)
+        self.V.update(samples)
+        self.V_unvisited.update(samples)
+        self.V_unvisited.add(self.x_goal)
+        self.V_open.add(self.x_init)
+
+    def Planning(self):
+        self.Init()
+        z = self.x_init
+        n = self.sample_numbers
+        rn = self.search_radius * math.sqrt((math.log(n) / n))
+        Visited = []
+
+        while z is not self.x_goal:
+            V_open_new = set()
+            X_near = self.Near(self.V_unvisited, z, rn)
+            Visited.append(z)
+
+            for x in X_near:
+                Y_near = self.Near(self.V_open, x, rn)
+                cost_list = {y: y.cost + self.Cost(y, x) for y in Y_near}
+                y_min = min(cost_list, key=cost_list.get)
+
+                if not self.utils.is_collision(y_min, x):
+                    x.parent = y_min
+                    V_open_new.add(x)
+                    self.V_unvisited.remove(x)
+                    x.cost = y_min.cost + self.Cost(y_min, x)
+
+            self.V_open.update(V_open_new)
+            self.V_open.remove(z)
+            self.V_closed.add(z)
+
+            if not self.V_open:
+                print("open set empty!")
+                break
+
+            cost_open = {y: y.cost for y in self.V_open}
+            z = min(cost_open, key=cost_open.get)
+
+        # node_end = self.ChooseGoalPoint()
+        path_x, path_y = self.ExtractPath()
+        self.animation(path_x, path_y, Visited[1: len(Visited)])
+
+    def ChooseGoalPoint(self):
+        Near = self.Near(self.V, self.x_goal, 2.0)
+        cost = {y: y.cost + self.Cost(y, self.x_goal) for y in Near}
+
+        return min(cost, key=cost.get)
+
+    def ExtractPath(self):
+        path_x, path_y = [], []
+        node = self.x_goal
+
+        while node.parent:
+            path_x.append(node.x)
+            path_y.append(node.y)
+            node = node.parent
+
+        path_x.append(self.x_init.x)
+        path_y.append(self.x_init.y)
+
+        return path_x, path_y
+
+    def Cost(self, x_start, x_end):
+        if self.utils.is_collision(x_start, x_end):
+            return np.inf
+        else:
+            return self.calc_dist(x_start, x_end)
+
+    @staticmethod
+    def calc_dist(x_start, x_end):
+        return math.hypot(x_start.x - x_end.x, x_start.y - x_end.y)
+
+    @staticmethod
+    def Near(nodelist, z, rn):
+        return {nd for nd in nodelist
+                if 0 < (nd.x - z.x) ** 2 + (nd.y - z.y) ** 2 <= rn ** 2}
+
+    def SampleFree(self):
+        n = self.sample_numbers
+        delta = self.utils.delta
+        Sample = set()
+
+        ind = 0
+        while ind < n:
+            node = Node((random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+                         random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
+            if self.utils.is_inside_obs(node):
+                continue
+            else:
+                Sample.add(node)
+                ind += 1
+
+        return Sample
+
+    def animation(self, path_x, path_y, visited):
+        self.plot_grid("Fast Marching Trees (FMT*)")
+
+        for node in self.V:
+            plt.plot(node.x, node.y, marker='.', color='lightgrey', markersize=3)
+
+        count = 0
+        for node in visited:
+            count += 1
+            plt.plot([node.x, node.parent.x], [node.y, node.parent.y], '-g')
+            plt.gcf().canvas.mpl_connect(
+                'key_release_event',
+                lambda event: [exit(0) if event.key == 'escape' else None])
+            if count % 10 == 0:
+                plt.pause(0.001)
+
+        plt.plot(path_x, path_y, linewidth=2, color='red')
+        plt.pause(0.01)
+        plt.show()
+
+    def plot_grid(self, name):
+
+        for (ox, oy, w, h) in self.obs_boundary:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='black',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, w, h) in self.obs_rectangle:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, r) in self.obs_circle:
+            self.ax.add_patch(
+                patches.Circle(
+                    (ox, oy), r,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        plt.plot(self.x_init.x, self.x_init.y, "bs", linewidth=3)
+        plt.plot(self.x_goal.x, self.x_goal.y, "rs", linewidth=3)
+
+        plt.title(name)
+        plt.axis("equal")
+
+
+def main():
+    x_start = (18, 8)  # Starting node
+    x_goal = (37, 18)  # Goal node
+
+    fmt = FMT(x_start, x_goal, 40)
+    fmt.Planning()
+
+
+if __name__ == '__main__':
+    main()

Sampling_based_Planning/rrt_2D/informed_rrt_star.py

@@ -0,0 +1,305 @@
+"""
+INFORMED_RRT_STAR 2D
+@author: huiming zhou
+"""
+
+import os
+import sys
+import math
+import random
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.spatial.transform import Rotation as Rot
+import matplotlib.patches as patches
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Sampling_based_Planning/")
+
+from Sampling_based_Planning.rrt_2D import env, plotting, utils
+
+
+class Node:
+    def __init__(self, n):
+        self.x = n[0]
+        self.y = n[1]
+        self.parent = None
+
+
+class IRrtStar:
+    def __init__(self, x_start, x_goal, step_len,
+                 goal_sample_rate, search_radius, iter_max):
+        self.x_start = Node(x_start)
+        self.x_goal = Node(x_goal)
+        self.step_len = step_len
+        self.goal_sample_rate = goal_sample_rate
+        self.search_radius = search_radius
+        self.iter_max = iter_max
+
+        self.env = env.Env()
+        self.plotting = plotting.Plotting(x_start, x_goal)
+        self.utils = utils.Utils()
+
+        self.fig, self.ax = plt.subplots()
+        self.delta = self.utils.delta
+        self.x_range = self.env.x_range
+        self.y_range = self.env.y_range
+        self.obs_circle = self.env.obs_circle
+        self.obs_rectangle = self.env.obs_rectangle
+        self.obs_boundary = self.env.obs_boundary
+
+        self.V = [self.x_start]
+        self.X_soln = set()
+        self.path = None
+
+    def init(self):
+        cMin, theta = self.get_distance_and_angle(self.x_start, self.x_goal)
+        C = self.RotationToWorldFrame(self.x_start, self.x_goal, cMin)
+        xCenter = np.array([[(self.x_start.x + self.x_goal.x) / 2.0],
+                            [(self.x_start.y + self.x_goal.y) / 2.0], [0.0]])
+        x_best = self.x_start
+
+        return theta, cMin, xCenter, C, x_best
+
+    def planning(self):
+        theta, dist, x_center, C, x_best = self.init()
+        c_best = np.inf
+
+        for k in range(self.iter_max):
+            if self.X_soln:
+                cost = {node: self.Cost(node) for node in self.X_soln}
+                x_best = min(cost, key=cost.get)
+                c_best = cost[x_best]
+
+            x_rand = self.Sample(c_best, dist, x_center, C)
+            x_nearest = self.Nearest(self.V, x_rand)
+            x_new = self.Steer(x_nearest, x_rand)
+
+            if x_new and not self.utils.is_collision(x_nearest, x_new):
+                X_near = self.Near(self.V, x_new)
+                c_min = self.Cost(x_nearest) + self.Line(x_nearest, x_new)
+                self.V.append(x_new)
+
+                # choose parent
+                for x_near in X_near:
+                    c_new = self.Cost(x_near) + self.Line(x_near, x_new)
+                    if c_new < c_min:
+                        x_new.parent = x_near
+                        c_min = c_new
+
+                # rewire
+                for x_near in X_near:
+                    c_near = self.Cost(x_near)
+                    c_new = self.Cost(x_new) + self.Line(x_new, x_near)
+                    if c_new < c_near:
+                        x_near.parent = x_new
+
+                if self.InGoalRegion(x_new):
+                    if not self.utils.is_collision(x_new, self.x_goal):
+                        self.X_soln.add(x_new)
+                        # new_cost = self.Cost(x_new) + self.Line(x_new, self.x_goal)
+                        # if new_cost < c_best:
+                        #     c_best = new_cost
+                        #     x_best = x_new
+
+            if k % 20 == 0:
+                self.animation(x_center=x_center, c_best=c_best, dist=dist, theta=theta)
+
+        self.path = self.ExtractPath(x_best)
+        self.animation(x_center=x_center, c_best=c_best, dist=dist, theta=theta)
+        plt.plot([x for x, _ in self.path], [y for _, y in self.path], '-r')
+        plt.pause(0.01)
+        plt.show()
+
+    def Steer(self, x_start, x_goal):
+        dist, theta = self.get_distance_and_angle(x_start, x_goal)
+        dist = min(self.step_len, dist)
+        node_new = Node((x_start.x + dist * math.cos(theta),
+                         x_start.y + dist * math.sin(theta)))
+        node_new.parent = x_start
+
+        return node_new
+
+    def Near(self, nodelist, node):
+        n = len(nodelist) + 1
+        r = 50 * math.sqrt((math.log(n) / n))
+
+        dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist]
+        X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2 and
+                  not self.utils.is_collision(nodelist[ind], node)]
+
+        return X_near
+
+    def Sample(self, c_max, c_min, x_center, C):
+        if c_max < np.inf:
+            r = [c_max / 2.0,
+                 math.sqrt(c_max ** 2 - c_min ** 2) / 2.0,
+                 math.sqrt(c_max ** 2 - c_min ** 2) / 2.0]
+            L = np.diag(r)
+
+            while True:
+                x_ball = self.SampleUnitBall()
+                x_rand = np.dot(np.dot(C, L), x_ball) + x_center
+                if self.x_range[0] + self.delta <= x_rand[0] <= self.x_range[1] - self.delta and \
+                        self.y_range[0] + self.delta <= x_rand[1] <= self.y_range[1] - self.delta:
+                    break
+            x_rand = Node((x_rand[(0, 0)], x_rand[(1, 0)]))
+        else:
+            x_rand = self.SampleFreeSpace()
+
+        return x_rand
+
+    @staticmethod
+    def SampleUnitBall():
+        while True:
+            x, y = random.uniform(-1, 1), random.uniform(-1, 1)
+            if x ** 2 + y ** 2 < 1:
+                return np.array([[x], [y], [0.0]])
+
+    def SampleFreeSpace(self):
+        delta = self.delta
+
+        if np.random.random() > self.goal_sample_rate:
+            return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+                         np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
+
+        return self.x_goal
+
+    def ExtractPath(self, node):
+        path = [[self.x_goal.x, self.x_goal.y]]
+
+        while node.parent:
+            path.append([node.x, node.y])
+            node = node.parent
+
+        path.append([self.x_start.x, self.x_start.y])
+
+        return path
+
+    def InGoalRegion(self, node):
+        if self.Line(node, self.x_goal) < self.step_len:
+            return True
+
+        return False
+
+    @staticmethod
+    def RotationToWorldFrame(x_start, x_goal, L):
+        a1 = np.array([[(x_goal.x - x_start.x) / L],
+                       [(x_goal.y - x_start.y) / L], [0.0]])
+        e1 = np.array([[1.0], [0.0], [0.0]])
+        M = a1 @ e1.T
+        U, _, V_T = np.linalg.svd(M, True, True)
+        C = U @ np.diag([1.0, 1.0, np.linalg.det(U) * np.linalg.det(V_T.T)]) @ V_T
+
+        return C
+
+    @staticmethod
+    def Nearest(nodelist, n):
+        return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2
+                                       for nd in nodelist]))]
+
+    @staticmethod
+    def Line(x_start, x_goal):
+        return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y)
+
+    def Cost(self, node):
+        if node == self.x_start:
+            return 0.0
+
+        if node.parent is None:
+            return np.inf
+
+        cost = 0.0
+        while node.parent:
+            cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
+            node = node.parent
+
+        return cost
+
+    @staticmethod
+    def get_distance_and_angle(node_start, node_end):
+        dx = node_end.x - node_start.x
+        dy = node_end.y - node_start.y
+        return math.hypot(dx, dy), math.atan2(dy, dx)
+
+    def animation(self, x_center=None, c_best=None, dist=None, theta=None):
+        plt.cla()
+        self.plot_grid("Informed rrt*, N = " + str(self.iter_max))
+        plt.gcf().canvas.mpl_connect(
+            'key_release_event',
+            lambda event: [exit(0) if event.key == 'escape' else None])
+
+        for node in self.V:
+            if node.parent:
+                plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")
+
+        if c_best != np.inf:
+            self.draw_ellipse(x_center, c_best, dist, theta)
+
+        plt.pause(0.01)
+
+    def plot_grid(self, name):
+
+        for (ox, oy, w, h) in self.obs_boundary:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='black',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, w, h) in self.obs_rectangle:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, r) in self.obs_circle:
+            self.ax.add_patch(
+                patches.Circle(
+                    (ox, oy), r,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        plt.plot(self.x_start.x, self.x_start.y, "bs", linewidth=3)
+        plt.plot(self.x_goal.x, self.x_goal.y, "rs", linewidth=3)
+
+        plt.title(name)
+        plt.axis("equal")
+
+    @staticmethod
+    def draw_ellipse(x_center, c_best, dist, theta):
+        a = math.sqrt(c_best ** 2 - dist ** 2) / 2.0
+        b = c_best / 2.0
+        angle = math.pi / 2.0 - theta
+        cx = x_center[0]
+        cy = x_center[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]
+        rot = Rot.from_euler('z', -angle).as_dcm()[0:2, 0:2]
+        fx = rot @ np.array([x, y])
+        px = np.array(fx[0, :] + cx).flatten()
+        py = np.array(fx[1, :] + cy).flatten()
+        plt.plot(cx, cy, ".b")
+        plt.plot(px, py, linestyle='--', color='darkorange', linewidth=2)
+
+
+def main():
+    x_start = (18, 8)  # Starting node
+    x_goal = (37, 18)  # Goal node
+
+    rrt_star = IRrtStar(x_start, x_goal, 1, 0.10, 12, 1000)
+    rrt_star.planning()
+
+
+if __name__ == '__main__':
+    main()

Sampling-based Planning/rrt_2D/plotting.py → Sampling_based_Planning/rrt_2D/plotting.py

@@ -9,9 +9,9 @@ import os
 import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
-                "/../../Sampling-based Planning/")
+                "/../../Sampling_based_Planning/")
 
-from rrt_2D import env
+from Sampling_based_Planning.rrt_2D import env
 
 
 class Plotting:
@@ -80,7 +80,8 @@ class Plotting:
                 if node.parent:
                     plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
                     plt.gcf().canvas.mpl_connect('key_release_event',
-                                                 lambda event: [exit(0) if event.key == 'escape' else None])
+                                                 lambda event:
+                                                 [exit(0) if event.key == 'escape' else None])
                     if count % 10 == 0:
                         plt.pause(0.001)
         else:
@@ -110,6 +111,7 @@ class Plotting:
 
     @staticmethod
     def plot_path(path):
-        plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2)
-        plt.pause(0.01)
+        if len(path) != 0:
+            plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2)
+            plt.pause(0.01)
         plt.show()

Sampling-based Planning/rrt_2D/rrt.py → Sampling_based_Planning/rrt_2D/rrt.py

@@ -9,11 +9,9 @@ import math
 import numpy as np
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
-                "/../../Sampling-based Planning/")
+                "/../../Sampling_based_Planning/")
 
-from rrt_2D import env
-from rrt_2D import plotting
-from rrt_2D import utils
+from Sampling_based_Planning.rrt_2D import env, plotting, utils
 
 
 class Node:
@@ -54,6 +52,7 @@ class Rrt:
 
                 if dist <= self.step_len:
                     self.new_state(node_new, self.s_goal)
+                    print(i)
                     return self.extract_path(node_new)
 
         return None
@@ -103,11 +102,11 @@ def main():
     x_start = (2, 2)  # Starting node
     x_goal = (49, 24)  # Goal node
 
-    rrt = Rrt(x_start, x_goal, 0.5, 0.03, 5000)
+    rrt = Rrt(x_start, x_goal, 0.5, 0.00, 10000)
     path = rrt.planning()
 
     if path:
-        rrt.plotting.animation(rrt.vertex, path, "Goal-bias RRT", True)
+        rrt.plotting.animation(rrt.vertex, path, "RRT", True)
     else:
         print("No Path Found!")
 

Sampling-based Planning/rrt_2D/rrt_connect.py → Sampling_based_Planning/rrt_2D/rrt_connect.py

@@ -11,11 +11,9 @@ import numpy as np
 import matplotlib.pyplot as plt
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
-                "/../../Sampling-based Planning/")
+                "/../../Sampling_based_Planning/")
 
-from rrt_2D import env
-from rrt_2D import plotting
-from rrt_2D import utils
+from Sampling_based_Planning.rrt_2D import env, plotting, utils
 
 
 class Node:

Sampling-based Planning/rrt_2D/rrt_star.py → Sampling_based_Planning/rrt_2D/rrt_star.py

@@ -9,31 +9,29 @@ import math
 import numpy as np
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
-                "/../../Sampling-based Planning/")
+                "/../../Sampling_based_Planning/")
 
-from rrt_2D import env
-from rrt_2D import plotting
-from rrt_2D import utils
+from Sampling_based_Planning.rrt_2D import env, plotting, utils, queue
 
 
 class Node:
     def __init__(self, n):
         self.x = n[0]
         self.y = n[1]
-        self.cost = 0.0
         self.parent = None
 
 
 class RrtStar:
     def __init__(self, x_start, x_goal, step_len,
                  goal_sample_rate, search_radius, iter_max):
-        self.xI = Node(x_start)
-        self.xG = Node(x_goal)
+        self.s_start = Node(x_start)
+        self.s_goal = Node(x_goal)
         self.step_len = step_len
         self.goal_sample_rate = goal_sample_rate
         self.search_radius = search_radius
         self.iter_max = iter_max
-        self.vertex = [self.xI]
+        self.vertex = [self.s_start]
+        self.path = []
 
         self.env = env.Env()
         self.plotting = plotting.Plotting(x_start, x_goal)
@@ -47,36 +45,25 @@ class RrtStar:
 
     def planning(self):
         for k in range(self.iter_max):
-            if k % 500 == 0:
-                print(k)
-
             node_rand = self.generate_random_node(self.goal_sample_rate)
             node_near = self.nearest_neighbor(self.vertex, node_rand)
             node_new = self.new_state(node_near, node_rand)
 
+            if k % 500 == 0:
+                print(k)
+
             if node_new and not self.utils.is_collision(node_near, node_new):
-                self.vertex.append(node_new)
                 neighbor_index = self.find_near_neighbor(node_new)
+                self.vertex.append(node_new)
+
                 if neighbor_index:
-                    node_new = self.choose_parent(node_new, neighbor_index)
-                    self.vertex.append(node_new)
+                    self.choose_parent(node_new, neighbor_index)
                     self.rewire(node_new, neighbor_index)
 
         index = self.search_goal_parent()
-        return self.extract_path(self.vertex[index])
-
-    def generate_random_node(self, goal_sample_rate):
-        delta = self.utils.delta
-
-        if np.random.random() > goal_sample_rate:
-            return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
-                         np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
+        self.path = self.extract_path(self.vertex[index])
 
-        return self.xG
-
-    def nearest_neighbor(self, node_list, n):
-        return self.vertex[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
-                                          for nd in node_list]))]
+        self.plotting.animation(self.vertex, self.path, "rrt*, N = " + str(self.iter_max))
 
     def new_state(self, node_start, node_goal):
         dist, theta = self.get_distance_and_angle(node_start, node_goal)
@@ -84,67 +71,93 @@ class RrtStar:
         dist = min(self.step_len, dist)
         node_new = Node((node_start.x + dist * math.cos(theta),
                          node_start.y + dist * math.sin(theta)))
+
         node_new.parent = node_start
 
         return node_new
 
+    def choose_parent(self, node_new, neighbor_index):
+        cost = [self.get_new_cost(self.vertex[i], node_new) for i in neighbor_index]
+
+        cost_min_index = neighbor_index[int(np.argmin(cost))]
+        node_new.parent = self.vertex[cost_min_index]
+
+    def rewire(self, node_new, neighbor_index):
+        for i in neighbor_index:
+            node_neighbor = self.vertex[i]
+
+            if self.cost(node_neighbor) > self.get_new_cost(node_new, node_neighbor):
+                node_neighbor.parent = node_new
+
+    def search_goal_parent(self):
+        dist_list = [math.hypot(n.x - self.s_goal.x, n.y - self.s_goal.y) for n in self.vertex]
+        node_index = [i for i in range(len(dist_list)) if dist_list[i] <= self.step_len]
+
+        if len(node_index) > 0:
+            cost_list = [dist_list[i] + self.cost(self.vertex[i]) for i in node_index
+                         if not self.utils.is_collision(self.vertex[i], self.s_goal)]
+            return node_index[int(np.argmin(cost_list))]
+
+        return len(self.vertex) - 1
+
+    def get_new_cost(self, node_start, node_end):
+        dist, _ = self.get_distance_and_angle(node_start, node_end)
+
+        return self.cost(node_start) + dist
+
+    def generate_random_node(self, goal_sample_rate):
+        delta = self.utils.delta
+
+        if np.random.random() > goal_sample_rate:
+            return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+                         np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
+
+        return self.s_goal
+
     def find_near_neighbor(self, node_new):
         n = len(self.vertex) + 1
         r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len)
 
         dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex]
-        dist_table_index = [dist_table.index(d) for d in dist_table if d <= r]
-        dist_table_index = [ind for ind in dist_table_index
-                            if not self.utils.is_collision(node_new, self.vertex[ind])]
+        dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and
+                            not self.utils.is_collision(node_new, self.vertex[ind])]
 
         return dist_table_index
 
-    def choose_parent(self, node_new, neighbor_index):
-        cost = []
-
-        for i in neighbor_index:
-            node_neighbor = self.vertex[i]
-            cost.append(self.get_new_cost(node_neighbor, node_new))
-
-        cost_min_index = neighbor_index[int(np.argmin(cost))]
-        node_new = self.new_state(self.vertex[cost_min_index], node_new)
-        node_new.cost = min(cost)
-
-        return node_new
+    @staticmethod
+    def nearest_neighbor(node_list, n):
+        return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
+                                        for nd in node_list]))]
 
-    def search_goal_parent(self):
-        dist_list = [math.hypot(n.x - self.xG.x, n.y - self.xG.y) for n in self.vertex]
-        node_index = [dist_list.index(i) for i in dist_list if i <= self.step_len]
+    @staticmethod
+    def cost(node_p):
+        node = node_p
+        cost = 0.0
 
-        if node_index:
-            cost_list = [dist_list[i] + self.vertex[i].cost for i in node_index
-                         if not self.utils.is_collision(self.vertex[i], self.xG)]
-            return node_index[int(np.argmin(cost_list))]
+        while node.parent:
+            cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
+            node = node.parent
 
-        return None
+        return cost
 
-    def rewire(self, node_new, neighbor_index):
-        for i in neighbor_index:
-            node_neighbor = self.vertex[i]
-            new_cost = self.get_new_cost(node_new, node_neighbor)
+    def update_cost(self, parent_node):
+        OPEN = queue.QueueFIFO()
+        OPEN.put(parent_node)
 
-            if node_neighbor.cost > new_cost:
-                self.vertex[i] = self.new_state(node_new, node_neighbor)
-                self.propagate_cost_to_leaves(node_new)
+        while not OPEN.empty():
+            node = OPEN.get()
 
-    def get_new_cost(self, node_start, node_end):
-        dist, _ = self.get_distance_and_angle(node_start, node_end)
-        return node_start.cost + dist
+            if len(node.child) == 0:
+                continue
 
-    def propagate_cost_to_leaves(self, parent_node):
-        for node in self.vertex:
-            if node.parent == parent_node:
-                node.cost = self.get_new_cost(parent_node, node)
-                self.propagate_cost_to_leaves(node)
+            for node_c in node.child:
+                node_c.Cost = self.get_new_cost(node, node_c)
+                OPEN.put(node_c)
 
     def extract_path(self, node_end):
-        path = [[self.xG.x, self.xG.y]]
+        path = [[self.s_goal.x, self.s_goal.y]]
         node = node_end
+
         while node.parent is not None:
             path.append([node.x, node.y])
             node = node.parent
@@ -160,16 +173,11 @@ class RrtStar:
 
 
 def main():
-    x_start = (2, 2)  # Starting node
-    x_goal = (49, 24)  # Goal node
+    x_start = (18, 8)  # Starting node
+    x_goal = (37, 18)  # Goal node
 
     rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 10000)
-    path = rrt_star.planning()
-
-    if path:
-        rrt_star.plotting.animation(rrt_star.vertex, path, "RRT*")
-    else:
-        print("No Path Found!")
+    rrt_star.planning()
 
 
 if __name__ == '__main__':

Sampling_based_Planning/rrt_2D/rrt_star_smart.py

@@ -0,0 +1,311 @@
+"""
+RRT_STAR_SMART 2D
+@author: huiming zhou
+"""
+
+import os
+import sys
+import math
+import random
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+from scipy.spatial.transform import Rotation as Rot
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Sampling_based_Planning/")
+
+from Sampling_based_Planning.rrt_2D import env, plotting, utils
+
+
+class Node:
+    def __init__(self, n):
+        self.x = n[0]
+        self.y = n[1]
+        self.parent = None
+
+
+class RrtStarSmart:
+    def __init__(self, x_start, x_goal, step_len,
+                 goal_sample_rate, search_radius, iter_max):
+        self.x_start = Node(x_start)
+        self.x_goal = Node(x_goal)
+        self.step_len = step_len
+        self.goal_sample_rate = goal_sample_rate
+        self.search_radius = search_radius
+        self.iter_max = iter_max
+
+        self.env = env.Env()
+        self.plotting = plotting.Plotting(x_start, x_goal)
+        self.utils = utils.Utils()
+
+        self.fig, self.ax = plt.subplots()
+        self.delta = self.utils.delta
+        self.x_range = self.env.x_range
+        self.y_range = self.env.y_range
+        self.obs_circle = self.env.obs_circle
+        self.obs_rectangle = self.env.obs_rectangle
+        self.obs_boundary = self.env.obs_boundary
+
+        self.V = [self.x_start]
+        self.beacons = []
+        self.beacons_radius = 2
+        self.direct_cost_old = np.inf
+        self.obs_vertex = self.utils.get_obs_vertex()
+        self.path = None
+
+    def planning(self):
+        n = 0
+        b = 2
+        InitPathFlag = False
+        self.ReformObsVertex()
+
+        for k in range(self.iter_max):
+            if k % 200 == 0:
+                print(k)
+
+            if (k - n) % b == 0 and len(self.beacons) > 0:
+                x_rand = self.Sample(self.beacons)
+            else:
+                x_rand = self.Sample()
+
+            x_nearest = self.Nearest(self.V, x_rand)
+            x_new = self.Steer(x_nearest, x_rand)
+
+            if x_new and not self.utils.is_collision(x_nearest, x_new):
+                X_near = self.Near(self.V, x_new)
+                self.V.append(x_new)
+
+                if X_near:
+                    # choose parent
+                    cost_list = [self.Cost(x_near) + self.Line(x_near, x_new) for x_near in X_near]
+                    x_new.parent = X_near[int(np.argmin(cost_list))]
+
+                    # rewire
+                    c_min = self.Cost(x_new)
+                    for x_near in X_near:
+                        c_near = self.Cost(x_near)
+                        c_new = c_min + self.Line(x_new, x_near)
+                        if c_new < c_near:
+                            x_near.parent = x_new
+
+                if not InitPathFlag and self.InitialPathFound(x_new):
+                    InitPathFlag = True
+                    n = k
+
+                if InitPathFlag:
+                    self.PathOptimization(x_new)
+                if k % 5 == 0:
+                    self.animation()
+
+        self.path = self.ExtractPath()
+        self.animation()
+        plt.plot([x for x, _ in self.path], [y for _, y in self.path], '-r')
+        plt.pause(0.01)
+        plt.show()
+
+    def PathOptimization(self, node):
+        direct_cost_new = 0.0
+        node_end = self.x_goal
+
+        while node.parent:
+            node_parent = node.parent
+            if not self.utils.is_collision(node_parent, node_end):
+                node_end.parent = node_parent
+            else:
+                direct_cost_new += self.Line(node, node_end)
+                node_end = node
+
+            node = node_parent
+
+        if direct_cost_new < self.direct_cost_old:
+            self.direct_cost_old = direct_cost_new
+            self.UpdateBeacons()
+
+    def UpdateBeacons(self):
+        node = self.x_goal
+        beacons = []
+
+        while node.parent:
+            near_vertex = [v for v in self.obs_vertex
+                           if (node.x - v[0]) ** 2 + (node.y - v[1]) ** 2 < 9]
+            if len(near_vertex) > 0:
+                for v in near_vertex:
+                    beacons.append(v)
+
+            node = node.parent
+
+        self.beacons = beacons
+
+    def ReformObsVertex(self):
+        obs_vertex = []
+
+        for obs in self.obs_vertex:
+            for vertex in obs:
+                obs_vertex.append(vertex)
+
+        self.obs_vertex = obs_vertex
+
+    def Steer(self, x_start, x_goal):
+        dist, theta = self.get_distance_and_angle(x_start, x_goal)
+        dist = min(self.step_len, dist)
+        node_new = Node((x_start.x + dist * math.cos(theta),
+                         x_start.y + dist * math.sin(theta)))
+        node_new.parent = x_start
+
+        return node_new
+
+    def Near(self, nodelist, node):
+        n = len(self.V) + 1
+        r = 50 * math.sqrt((math.log(n) / n))
+
+        dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist]
+        X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2 and
+                  not self.utils.is_collision(node, nodelist[ind])]
+
+        return X_near
+
+    def Sample(self, goal=None):
+        if goal is None:
+            delta = self.utils.delta
+            goal_sample_rate = self.goal_sample_rate
+
+            if np.random.random() > goal_sample_rate:
+                return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+                             np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
+
+            return self.x_goal
+        else:
+            R = self.beacons_radius
+            r = random.uniform(0, R)
+            theta = random.uniform(0, 2 * math.pi)
+            ind = random.randint(0, len(goal) - 1)
+
+            return Node((goal[ind][0] + r * math.cos(theta),
+                         goal[ind][1] + r * math.sin(theta)))
+
+    def SampleFreeSpace(self):
+        delta = self.delta
+
+        if np.random.random() > self.goal_sample_rate:
+            return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+                         np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
+
+        return self.x_goal
+
+    def ExtractPath(self):
+        path = []
+        node = self.x_goal
+
+        while node.parent:
+            path.append([node.x, node.y])
+            node = node.parent
+
+        path.append([self.x_start.x, self.x_start.y])
+
+        return path
+
+    def InitialPathFound(self, node):
+        if self.Line(node, self.x_goal) < self.step_len:
+            return True
+
+        return False
+
+    @staticmethod
+    def Nearest(nodelist, n):
+        return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2
+                                       for nd in nodelist]))]
+
+    @staticmethod
+    def Line(x_start, x_goal):
+        return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y)
+
+    @staticmethod
+    def Cost(node):
+        cost = 0.0
+        if node.parent is None:
+            return cost
+
+        while node.parent:
+            cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
+            node = node.parent
+
+        return cost
+
+    @staticmethod
+    def get_distance_and_angle(node_start, node_end):
+        dx = node_end.x - node_start.x
+        dy = node_end.y - node_start.y
+        return math.hypot(dx, dy), math.atan2(dy, dx)
+
+    def animation(self):
+        plt.cla()
+        self.plot_grid("rrt*-Smart, N = " + str(self.iter_max))
+        plt.gcf().canvas.mpl_connect(
+            'key_release_event',
+            lambda event: [exit(0) if event.key == 'escape' else None])
+
+        for node in self.V:
+            if node.parent:
+                plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")
+
+        if self.beacons:
+            theta = np.arange(0, 2 * math.pi, 0.1)
+            r = self.beacons_radius
+
+            for v in self.beacons:
+                x = v[0] + r * np.cos(theta)
+                y = v[1] + r * np.sin(theta)
+                plt.plot(x, y, linestyle='--', linewidth=2, color='darkorange')
+
+        plt.pause(0.01)
+
+    def plot_grid(self, name):
+
+        for (ox, oy, w, h) in self.obs_boundary:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='black',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, w, h) in self.obs_rectangle:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, r) in self.obs_circle:
+            self.ax.add_patch(
+                patches.Circle(
+                    (ox, oy), r,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        plt.plot(self.x_start.x, self.x_start.y, "bs", linewidth=3)
+        plt.plot(self.x_goal.x, self.x_goal.y, "rs", linewidth=3)
+
+        plt.title(name)
+        plt.axis("equal")
+
+
+def main():
+    x_start = (18, 8)  # Starting node
+    x_goal = (37, 18)  # Goal node
+
+    rrt = RrtStarSmart(x_start, x_goal, 1.5, 0.10, 0, 1000)
+    rrt.planning()
+
+
+if __name__ == '__main__':
+    main()

Sampling-based Planning/rrt_2D/utils.py → Sampling_based_Planning/rrt_2D/utils.py

@@ -9,10 +9,10 @@ import os
 import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
-                "/../../Sampling-based Planning/")
+                "/../../Sampling_based_Planning/")
 
-from rrt_2D import env
-from rrt_2D.rrt import Node
+from Sampling_based_Planning.rrt_2D import env
+from Sampling_based_Planning.rrt_2D.rrt import Node
 
 
 class Utils:

Sampling-based Planning/rrt_3D/dynamic_rrt3D.py → Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py

@@ -12,7 +12,7 @@ import matplotlib.pyplot as plt
 import os
 import sys
 
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based_Planning/")
 from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, cost, path, edgeset, isinbound, isinside
 from rrt_3D.rrt3D import rrt

Sampling-based Planning/rrt_3D/extend_rrt3D.py → Sampling_based_Planning/rrt_3D/extend_rrt3D.py

@@ -12,7 +12,7 @@ import matplotlib.pyplot as plt
 import os
 import sys
 
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/")
 from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path
 

Sampling-based Planning/rrt_3D/rrt3D.py → Sampling_based_Planning/rrt_3D/rrt3D.py

@@ -11,7 +11,7 @@ import matplotlib.pyplot as plt
 import os
 import sys
 
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/")
 
 from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path

Sampling-based Planning/rrt_3D/rrt_connect3D.py → Sampling_based_Planning/rrt_3D/rrt_connect3D.py

@@ -12,7 +12,7 @@ import matplotlib.pyplot as plt
 import os
 import sys
 
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/")
 
 from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset

Sampling-based Planning/rrt_3D/rrtstar3D.py → Sampling_based_Planning/rrt_3D/rrtstar3D.py

@@ -11,7 +11,7 @@ import matplotlib.pyplot as plt
 import os
 import sys
 
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/")
 from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path
 

Sampling-based Planning/rrt_3D/utils3D.py → Sampling_based_Planning/rrt_3D/utils3D.py

@@ -6,7 +6,7 @@ from collections import deque
 import os
 import sys
 
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/")
 from rrt_3D.plot_util3D import visualization
 
 

Search-based Planning/.idea/Search-based Planning.iml

@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<module type="PYTHON_MODULE" version="4">
-  <component name="NewModuleRootManager">
-    <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="Python 3.7" jdkType="Python SDK" />
-    <orderEntry type="sourceFolder" forTests="false" />
-  </component>
-</module>

Search-based Planning/.idea/dictionaries/Huiming_Zhou.xml

@@ -1,10 +0,0 @@
-<component name="ProjectDictionaryState">
-  <dictionary name="Huiming Zhou">
-    <words>
-      <w>astar</w>
-      <w>dijk</w>
-      <w>huiming</w>
-      <w>zhou</w>
-    </words>
-  </dictionary>
-</component>

Search-based Planning/.idea/inspectionProfiles/profiles_settings.xml

@@ -1,6 +0,0 @@
-<component name="InspectionProjectProfileManager">
-  <settings>
-    <option name="USE_PROJECT_PROFILE" value="false" />
-    <version value="1.0" />
-  </settings>
-</component>

Search-based Planning/.idea/misc.xml

@@ -1,4 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
-</project>

Search-based Planning/.idea/modules.xml

@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectModuleManager">
-    <modules>
-      <module fileurl="file://$PROJECT_DIR$/.idea/Search-based Planning.iml" filepath="$PROJECT_DIR$/.idea/Search-based Planning.iml" />
-    </modules>
-  </component>
-</project>

Search-based Planning/.idea/vcs.xml

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="VcsDirectoryMappings">
-    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
-  </component>
-</project>

Search-based Planning/.idea/workspace.xml

@@ -1,293 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="BranchesTreeState">
-    <expand>
-      <path>
-        <item name="ROOT" type="e8cecc67:BranchNodeDescriptor" />
-        <item name="LOCAL_ROOT" type="e8cecc67:BranchNodeDescriptor" />
-      </path>
-      <path>
-        <item name="ROOT" type="e8cecc67:BranchNodeDescriptor" />
-        <item name="REMOTE_ROOT" type="e8cecc67:BranchNodeDescriptor" />
-      </path>
-      <path>
-        <item name="ROOT" type="e8cecc67:BranchNodeDescriptor" />
-        <item name="REMOTE_ROOT" type="e8cecc67:BranchNodeDescriptor" />
-        <item name="GROUP_NODE:origin" type="e8cecc67:BranchNodeDescriptor" />
-      </path>
-    </expand>
-    <select />
-  </component>
-  <component name="ChangeListManager">
-    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
-      <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
-    </list>
-    <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
-    <option name="SHOW_DIALOG" value="false" />
-    <option name="HIGHLIGHT_CONFLICTS" value="true" />
-    <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
-    <option name="LAST_RESOLUTION" value="IGNORE" />
-  </component>
-  <component name="FileTemplateManagerImpl">
-    <option name="RECENT_TEMPLATES">
-      <list>
-        <option value="Python Script" />
-      </list>
-    </option>
-  </component>
-  <component name="Git.Settings">
-    <option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$/.." />
-  </component>
-  <component name="ProjectId" id="1dQBIivqkvFljqtAc1O2MqInYWf" />
-  <component name="ProjectLevelVcsManager" settingsEditedManually="true">
-    <ConfirmationsSetting value="2" id="Add" />
-  </component>
-  <component name="ProjectViewState">
-    <option name="hideEmptyMiddlePackages" value="true" />
-    <option name="showLibraryContents" value="true" />
-  </component>
-  <component name="PropertiesComponent">
-    <property name="ASKED_ADD_EXTERNAL_FILES" value="true" />
-    <property name="RunOnceActivity.OpenProjectViewOnStart" value="true" />
-    <property name="RunOnceActivity.ShowReadmeOnStart" value="true" />
-    <property name="last_opened_file_path" value="$PROJECT_DIR$" />
-    <property name="restartRequiresConfirmation" value="false" />
-    <property name="run.code.analysis.last.selected.profile" value="aDefault" />
-    <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
-  </component>
-  <component name="RecentsManager">
-    <key name="MoveFile.RECENT_KEYS">
-      <recent name="C:\Users\Huiming Zhou\Desktop\path planning algorithms\Search-based Planning\Search_2D" />
-    </key>
-  </component>
-  <component name="RunDashboard">
-    <option name="ruleStates">
-      <list>
-        <RuleState>
-          <option name="name" value="ConfigurationTypeDashboardGroupingRule" />
-        </RuleState>
-        <RuleState>
-          <option name="name" value="StatusDashboardGroupingRule" />
-        </RuleState>
-      </list>
-    </option>
-  </component>
-  <component name="RunManager" selected="Python.best_first">
-    <configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
-      <module name="Search-based Planning" />
-      <option name="INTERPRETER_OPTIONS" value="" />
-      <option name="PARENT_ENVS" value="true" />
-      <envs>
-        <env name="PYTHONUNBUFFERED" value="1" />
-      </envs>
-      <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
-      <option name="IS_MODULE_SDK" value="true" />
-      <option name="ADD_CONTENT_ROOTS" value="true" />
-      <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/ARAstar.py" />
-      <option name="PARAMETERS" value="" />
-      <option name="SHOW_COMMAND_LINE" value="false" />
-      <option name="EMULATE_TERMINAL" value="false" />
-      <option name="MODULE_MODE" value="false" />
-      <option name="REDIRECT_INPUT" value="false" />
-      <option name="INPUT_FILE" value="" />
-      <method v="2" />
-    </configuration>
-    <configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
-      <module name="Search-based Planning" />
-      <option name="INTERPRETER_OPTIONS" value="" />
-      <option name="PARENT_ENVS" value="true" />
-      <envs>
-        <env name="PYTHONUNBUFFERED" value="1" />
-      </envs>
-      <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
-      <option name="IS_MODULE_SDK" value="true" />
-      <option name="ADD_CONTENT_ROOTS" value="true" />
-      <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/D_star_Lite.py" />
-      <option name="PARAMETERS" value="" />
-      <option name="SHOW_COMMAND_LINE" value="false" />
-      <option name="EMULATE_TERMINAL" value="false" />
-      <option name="MODULE_MODE" value="false" />
-      <option name="REDIRECT_INPUT" value="false" />
-      <option name="INPUT_FILE" value="" />
-      <method v="2" />
-    </configuration>
-    <configuration name="Field_D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
-      <module name="Search-based Planning" />
-      <option name="INTERPRETER_OPTIONS" value="" />
-      <option name="PARENT_ENVS" value="true" />
-      <envs>
-        <env name="PYTHONUNBUFFERED" value="1" />
-      </envs>
-      <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
-      <option name="IS_MODULE_SDK" value="true" />
-      <option name="ADD_CONTENT_ROOTS" value="true" />
-      <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Field_D_star.py" />
-      <option name="PARAMETERS" value="" />
-      <option name="SHOW_COMMAND_LINE" value="false" />
-      <option name="EMULATE_TERMINAL" value="false" />
-      <option name="MODULE_MODE" value="false" />
-      <option name="REDIRECT_INPUT" value="false" />
-      <option name="INPUT_FILE" value="" />
-      <method v="2" />
-    </configuration>
-    <configuration name="Potential_Field" type="PythonConfigurationType" factoryName="Python" temporary="true">
-      <module name="Search-based Planning" />
-      <option name="INTERPRETER_OPTIONS" value="" />
-      <option name="PARENT_ENVS" value="true" />
-      <envs>
-        <env name="PYTHONUNBUFFERED" value="1" />
-      </envs>
-      <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
-      <option name="IS_MODULE_SDK" value="true" />
-      <option name="ADD_CONTENT_ROOTS" value="true" />
-      <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Potential_Field.py" />
-      <option name="PARAMETERS" value="" />
-      <option name="SHOW_COMMAND_LINE" value="false" />
-      <option name="EMULATE_TERMINAL" value="false" />
-      <option name="MODULE_MODE" value="false" />
-      <option name="REDIRECT_INPUT" value="false" />
-      <option name="INPUT_FILE" value="" />
-      <method v="2" />
-    </configuration>
-    <configuration name="best_first" type="PythonConfigurationType" factoryName="Python" temporary="true">
-      <module name="Search-based Planning" />
-      <option name="INTERPRETER_OPTIONS" value="" />
-      <option name="PARENT_ENVS" value="true" />
-      <envs>
-        <env name="PYTHONUNBUFFERED" value="1" />
-      </envs>
-      <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
-      <option name="IS_MODULE_SDK" value="true" />
-      <option name="ADD_CONTENT_ROOTS" value="true" />
-      <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/best_first.py" />
-      <option name="PARAMETERS" value="" />
-      <option name="SHOW_COMMAND_LINE" value="false" />
-      <option name="EMULATE_TERMINAL" value="false" />
-      <option name="MODULE_MODE" value="false" />
-      <option name="REDIRECT_INPUT" value="false" />
-      <option name="INPUT_FILE" value="" />
-      <method v="2" />
-    </configuration>
-    <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" nameIsGenerated="true">
-      <module name="Search-based Planning" />
-      <option name="INTERPRETER_OPTIONS" value="" />
-      <option name="PARENT_ENVS" value="true" />
-      <envs>
-        <env name="PYTHONUNBUFFERED" value="1" />
-      </envs>
-      <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
-      <option name="IS_MODULE_SDK" value="true" />
-      <option name="ADD_CONTENT_ROOTS" value="true" />
-      <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/dijkstra.py" />
-      <option name="PARAMETERS" value="" />
-      <option name="SHOW_COMMAND_LINE" value="false" />
-      <option name="EMULATE_TERMINAL" value="false" />
-      <option name="MODULE_MODE" value="false" />
-      <option name="REDIRECT_INPUT" value="false" />
-      <option name="INPUT_FILE" value="" />
-      <method v="2" />
-    </configuration>
-    <list>
-      <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.ARAstar" />
-      <item itemvalue="Python.Potential_Field" />
-      <item itemvalue="Python.D_star_Lite" />
-      <item itemvalue="Python.Field_D_star" />
-      <item itemvalue="Python.best_first" />
-    </list>
-    <recent_temporary>
-      <list>
-        <item itemvalue="Python.best_first" />
-        <item itemvalue="Python.Field_D_star" />
-        <item itemvalue="Python.D_star_Lite" />
-        <item itemvalue="Python.Potential_Field" />
-        <item itemvalue="Python.ARAstar" />
-      </list>
-    </recent_temporary>
-  </component>
-  <component name="SvnConfiguration">
-    <configuration />
-  </component>
-  <component name="TaskManager">
-    <task active="true" id="Default" summary="Default task">
-      <changelist id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="" />
-      <created>1592347358698</created>
-      <option name="number" value="Default" />
-      <option name="presentableId" value="Default" />
-      <updated>1592347358698</updated>
-    </task>
-    <task id="LOCAL-00001" summary="Merge branch 'master' of https://github.com/zhm-real/path-planning-algorithms&#10;&#10;# Please enter a commit message to explain why this merge is necessary,&#10;# especially if it merges an updated upstream into a topic branch.&#10;#&#10;# Lines starting with '#' will be ignored, and an empty message aborts&#10;# the commit.">
-      <created>1593715021929</created>
-      <option name="number" value="00001" />
-      <option name="presentableId" value="LOCAL-00001" />
-      <option name="project" value="LOCAL" />
-      <updated>1593715021929</updated>
-    </task>
-    <option name="localTasksCounter" value="2" />
-    <servers />
-  </component>
-  <component name="Vcs.Log.Tabs.Properties">
-    <option name="TAB_STATES">
-      <map>
-        <entry key="MAIN">
-          <value>
-            <State>
-              <option name="COLUMN_ORDER" />
-            </State>
-          </value>
-        </entry>
-      </map>
-    </option>
-  </component>
-  <component name="VcsManagerConfiguration">
-    <MESSAGE value="Merge branch 'master' of https://github.com/zhm-real/path-planning-algorithms&#10;&#10;# Please enter a commit message to explain why this merge is necessary,&#10;# especially if it merges an updated upstream into a topic branch.&#10;#&#10;# Lines starting with '#' will be ignored, and an empty message aborts&#10;# the commit." />
-    <option name="LAST_COMMIT_MESSAGE" value="Merge branch 'master' of https://github.com/zhm-real/path-planning-algorithms&#10;&#10;# Please enter a commit message to explain why this merge is necessary,&#10;# especially if it merges an updated upstream into a topic branch.&#10;#&#10;# Lines starting with '#' will be ignored, and an empty message aborts&#10;# the commit." />
-  </component>
-  <component name="WindowStateProjectService">
-    <state x="2377" y="197" key="#com.intellij.execution.impl.EditConfigurationsDialog" timestamp="1593282711181">
-      <screen x="1920" y="0" width="1920" height="1080" />
-    </state>
-    <state x="2377" y="197" key="#com.intellij.execution.impl.EditConfigurationsDialog/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282711181" />
-    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl" timestamp="1593631602696">
-      <screen x="1920" y="0" width="1920" height="1080" />
-    </state>
-    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593631602696" />
-    <state x="819" y="314" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@65.24.1855.1056" timestamp="1592933974409" />
-    <state width="1832" height="146" key="GridCell.Tab.0.bottom" timestamp="1593715356351">
-      <screen x="1920" y="0" width="1920" height="1080" />
-    </state>
-    <state width="1832" height="146" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593715356351" />
-    <state width="1832" height="146" key="GridCell.Tab.0.center" timestamp="1593715356351">
-      <screen x="1920" y="0" width="1920" height="1080" />
-    </state>
-    <state width="1832" height="146" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593715356351" />
-    <state width="1832" height="146" key="GridCell.Tab.0.left" timestamp="1593715356351">
-      <screen x="1920" y="0" width="1920" height="1080" />
-    </state>
-    <state width="1832" height="146" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593715356351" />
-    <state width="1832" height="146" key="GridCell.Tab.0.right" timestamp="1593715356351">
-      <screen x="1920" y="0" width="1920" height="1080" />
-    </state>
-    <state width="1832" height="146" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593715356351" />
-    <state x="2406" y="174" key="SettingsEditor" timestamp="1593282573348">
-      <screen x="1920" y="0" width="1920" height="1080" />
-    </state>
-    <state x="2406" y="174" key="SettingsEditor/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282573348" />
-    <state x="2701" y="438" key="com.intellij.openapi.vcs.update.UpdateOrStatusOptionsDialogupdate-v2" timestamp="1593715195096">
-      <screen x="1920" y="0" width="1920" height="1080" />
-    </state>
-    <state x="2701" y="438" key="com.intellij.openapi.vcs.update.UpdateOrStatusOptionsDialogupdate-v2/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593715195096" />
-  </component>
-</project>