diff --git a/.idea/.gitignore b/.idea/.gitignore
index 26d3352..0e40fe8 100644
--- a/.idea/.gitignore
+++ b/.idea/.gitignore
@@ -1,3 +1,3 @@
+
# Default ignored files
-/shelf/
-/workspace.xml
+/workspace.xml
\ No newline at end of file
diff --git a/Stochastic Shortest Path/.idea/Stochastic Shortest Path.iml b/.idea/PathPlanning.iml
similarity index 78%
rename from Stochastic Shortest Path/.idea/Stochastic Shortest Path.iml
rename to .idea/PathPlanning.iml
index 548113a..7c9d48f 100644
--- a/Stochastic Shortest Path/.idea/Stochastic Shortest Path.iml
+++ b/.idea/PathPlanning.iml
@@ -2,10 +2,11 @@
-
+
+
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
index e889cbd..d8bd288 100644
--- a/.idea/modules.xml
+++ b/.idea/modules.xml
@@ -2,7 +2,7 @@
-
+
\ No newline at end of file
diff --git a/.idea/path-planning-algorithms.iml b/.idea/path-planning-algorithms.iml
deleted file mode 100644
index 95f5d6e..0000000
--- a/.idea/path-planning-algorithms.iml
+++ /dev/null
@@ -1,11 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/CurvesGenerator/__pycache__/draw.cpython-37.pyc b/CurvesGenerator/__pycache__/draw.cpython-37.pyc
new file mode 100644
index 0000000..2abd05e
Binary files /dev/null and b/CurvesGenerator/__pycache__/draw.cpython-37.pyc differ
diff --git a/CurvesGenerator/__pycache__/dubins_path.cpython-37.pyc b/CurvesGenerator/__pycache__/dubins_path.cpython-37.pyc
new file mode 100644
index 0000000..945400f
Binary files /dev/null and b/CurvesGenerator/__pycache__/dubins_path.cpython-37.pyc differ
diff --git a/CurvesGenerator/bezier_path.py b/CurvesGenerator/bezier_path.py
new file mode 100644
index 0000000..f2f04de
--- /dev/null
+++ b/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()
diff --git a/CurvesGenerator/bspline_curve.py b/CurvesGenerator/bspline_curve.py
new file mode 100644
index 0000000..31a36d4
--- /dev/null
+++ b/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()
diff --git a/CurvesGenerator/cubic_spline.py b/CurvesGenerator/cubic_spline.py
new file mode 100644
index 0000000..d33b461
--- /dev/null
+++ b/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()
diff --git a/CurvesGenerator/draw.py b/CurvesGenerator/draw.py
new file mode 100644
index 0000000..9e8bd33
--- /dev/null
+++ b/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)
diff --git a/CurvesGenerator/dubins_path.py b/CurvesGenerator/dubins_path.py
new file mode 100644
index 0000000..33361fe
--- /dev/null
+++ b/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()
diff --git a/CurvesGenerator/quartic_polynomial.py b/CurvesGenerator/quartic_polynomial.py
new file mode 100644
index 0000000..d85cefb
--- /dev/null
+++ b/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
+
diff --git a/CurvesGenerator/quintic_polynomial.py b/CurvesGenerator/quintic_polynomial.py
new file mode 100644
index 0000000..378819a
--- /dev/null
+++ b/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()
diff --git a/CurvesGenerator/reeds_shepp.py b/CurvesGenerator/reeds_shepp.py
new file mode 100644
index 0000000..175def2
--- /dev/null
+++ b/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()
diff --git a/Model-free Control/.idea/.gitignore b/Model-free Control/.idea/.gitignore
deleted file mode 100644
index 0e40fe8..0000000
--- a/Model-free Control/.idea/.gitignore
+++ /dev/null
@@ -1,3 +0,0 @@
-
-# Default ignored files
-/workspace.xml
\ No newline at end of file
diff --git a/Model-free Control/.idea/Model-free Control.iml b/Model-free Control/.idea/Model-free Control.iml
deleted file mode 100644
index 5965bde..0000000
--- a/Model-free Control/.idea/Model-free Control.iml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Model-free Control/.idea/dictionaries/Huiming_Zhou.xml b/Model-free Control/.idea/dictionaries/Huiming_Zhou.xml
deleted file mode 100644
index a1d33a5..0000000
--- a/Model-free Control/.idea/dictionaries/Huiming_Zhou.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-
-
-
- sarsa
-
-
-
\ No newline at end of file
diff --git a/Model-free Control/.idea/inspectionProfiles/profiles_settings.xml b/Model-free Control/.idea/inspectionProfiles/profiles_settings.xml
deleted file mode 100644
index 105ce2d..0000000
--- a/Model-free Control/.idea/inspectionProfiles/profiles_settings.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Model-free Control/.idea/misc.xml b/Model-free Control/.idea/misc.xml
deleted file mode 100644
index 0e7ac62..0000000
--- a/Model-free Control/.idea/misc.xml
+++ /dev/null
@@ -1,4 +0,0 @@
-
-
-
-
\ No newline at end of file
diff --git a/Model-free Control/.idea/modules.xml b/Model-free Control/.idea/modules.xml
deleted file mode 100644
index 778e7db..0000000
--- a/Model-free Control/.idea/modules.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Model-free Control/.idea/vcs.xml b/Model-free Control/.idea/vcs.xml
deleted file mode 100644
index 6c0b863..0000000
--- a/Model-free Control/.idea/vcs.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Model-free Control/Q-learning.py b/Model-free Control/Q-learning.py
deleted file mode 100644
index bc428bf..0000000
--- a/Model-free Control/Q-learning.py
+++ /dev/null
@@ -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)
diff --git a/Model-free Control/Sarsa.py b/Model-free Control/Sarsa.py
deleted file mode 100644
index 2278a74..0000000
--- a/Model-free Control/Sarsa.py
+++ /dev/null
@@ -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)
diff --git a/Model-free Control/__pycache__/env.cpython-37.pyc b/Model-free Control/__pycache__/env.cpython-37.pyc
deleted file mode 100644
index 01d4899..0000000
Binary files a/Model-free Control/__pycache__/env.cpython-37.pyc and /dev/null differ
diff --git a/Model-free Control/__pycache__/motion_model.cpython-37.pyc b/Model-free Control/__pycache__/motion_model.cpython-37.pyc
deleted file mode 100644
index abf2ec9..0000000
Binary files a/Model-free Control/__pycache__/motion_model.cpython-37.pyc and /dev/null differ
diff --git a/Model-free Control/__pycache__/plotting.cpython-37.pyc b/Model-free Control/__pycache__/plotting.cpython-37.pyc
deleted file mode 100644
index b57e52b..0000000
Binary files a/Model-free Control/__pycache__/plotting.cpython-37.pyc and /dev/null differ
diff --git a/Model-free Control/env.py b/Model-free Control/env.py
deleted file mode 100644
index 8deed5d..0000000
--- a/Model-free Control/env.py
+++ /dev/null
@@ -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
diff --git a/Model-free Control/gif/Qlearning.gif b/Model-free Control/gif/Qlearning.gif
deleted file mode 100644
index d4d46bb..0000000
Binary files a/Model-free Control/gif/Qlearning.gif and /dev/null differ
diff --git a/Model-free Control/gif/SARSA.gif b/Model-free Control/gif/SARSA.gif
deleted file mode 100644
index 63be80d..0000000
Binary files a/Model-free Control/gif/SARSA.gif and /dev/null differ
diff --git a/Model-free Control/motion_model.py b/Model-free Control/motion_model.py
deleted file mode 100644
index 0285570..0000000
--- a/Model-free Control/motion_model.py
+++ /dev/null
@@ -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
diff --git a/Model-free Control/plotting.py b/Model-free Control/plotting.py
deleted file mode 100644
index 98f8d16..0000000
--- a/Model-free Control/plotting.py
+++ /dev/null
@@ -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()
diff --git a/README.md b/README.md
index 283b584..83b43d5 100644
--- a/README.md
+++ b/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
@@ -52,32 +50,32 @@ Directory Structure
@@ -87,45 +85,44 @@ Directory Structure
-
-### Value/Policy/Q-value/Q-policy Iteration
-* Brown: losing states
-
-
-### SARSA(on-policy) & Q-learning(off-policy)
-* Brown: losing states
-
+
+
+  |
+  |
## 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)
diff --git a/Sampling-based Planning/.idea/.gitignore b/Sampling-based Planning/.idea/.gitignore
deleted file mode 100644
index 26d3352..0000000
--- a/Sampling-based Planning/.idea/.gitignore
+++ /dev/null
@@ -1,3 +0,0 @@
-# Default ignored files
-/shelf/
-/workspace.xml
diff --git a/Sampling-based Planning/.idea/Sampling-based Planning.iml b/Sampling-based Planning/.idea/Sampling-based Planning.iml
deleted file mode 100644
index c444878..0000000
--- a/Sampling-based Planning/.idea/Sampling-based Planning.iml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Sampling-based Planning/.idea/dictionaries/zhou.xml b/Sampling-based Planning/.idea/dictionaries/zhou.xml
deleted file mode 100644
index 6638eb0..0000000
--- a/Sampling-based Planning/.idea/dictionaries/zhou.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-
-
- huiming
- zhou
-
-
-
\ No newline at end of file
diff --git a/Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml b/Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml
deleted file mode 100644
index 105ce2d..0000000
--- a/Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Sampling-based Planning/.idea/misc.xml b/Sampling-based Planning/.idea/misc.xml
deleted file mode 100644
index a2e120d..0000000
--- a/Sampling-based Planning/.idea/misc.xml
+++ /dev/null
@@ -1,4 +0,0 @@
-
-
-
-
\ No newline at end of file
diff --git a/Sampling-based Planning/.idea/modules.xml b/Sampling-based Planning/.idea/modules.xml
deleted file mode 100644
index caf2a07..0000000
--- a/Sampling-based Planning/.idea/modules.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Sampling-based Planning/.idea/vcs.xml b/Sampling-based Planning/.idea/vcs.xml
deleted file mode 100644
index 6c0b863..0000000
--- a/Sampling-based Planning/.idea/vcs.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Sampling-based Planning/gif/Goal_biasd_RRT_2D.gif b/Sampling-based Planning/gif/Goal_biasd_RRT_2D.gif
deleted file mode 100644
index d4a9340..0000000
Binary files a/Sampling-based Planning/gif/Goal_biasd_RRT_2D.gif and /dev/null differ
diff --git a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc
deleted file mode 100644
index bfd65b2..0000000
Binary files a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc and /dev/null differ
diff --git a/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc
deleted file mode 100644
index af6845f..0000000
Binary files a/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc and /dev/null differ
diff --git a/Sampling-based Planning/rrt_2D/gif/RRT_star.jpeg b/Sampling-based Planning/rrt_2D/gif/RRT_star.jpeg
deleted file mode 100644
index 1646ec8..0000000
Binary files a/Sampling-based Planning/rrt_2D/gif/RRT_star.jpeg and /dev/null differ
diff --git a/Sampling_based_Planning/gif/BIT.gif b/Sampling_based_Planning/gif/BIT.gif
new file mode 100644
index 0000000..649970f
Binary files /dev/null and b/Sampling_based_Planning/gif/BIT.gif differ
diff --git a/Sampling_based_Planning/gif/BIT2.gif b/Sampling_based_Planning/gif/BIT2.gif
new file mode 100644
index 0000000..973d278
Binary files /dev/null and b/Sampling_based_Planning/gif/BIT2.gif differ
diff --git a/Sampling-based Planning/gif/Dynamic_RRT_2D.gif b/Sampling_based_Planning/gif/Dynamic_RRT_2D.gif
similarity index 100%
rename from Sampling-based Planning/gif/Dynamic_RRT_2D.gif
rename to Sampling_based_Planning/gif/Dynamic_RRT_2D.gif
diff --git a/Sampling-based Planning/gif/Extended_RRT_2D.gif b/Sampling_based_Planning/gif/Extended_RRT_2D.gif
similarity index 100%
rename from Sampling-based Planning/gif/Extended_RRT_2D.gif
rename to Sampling_based_Planning/gif/Extended_RRT_2D.gif
diff --git a/Sampling_based_Planning/gif/FMT.gif b/Sampling_based_Planning/gif/FMT.gif
new file mode 100644
index 0000000..4648087
Binary files /dev/null and b/Sampling_based_Planning/gif/FMT.gif differ
diff --git a/Sampling_based_Planning/gif/Goal_biasd_RRT_2D.gif b/Sampling_based_Planning/gif/Goal_biasd_RRT_2D.gif
new file mode 100644
index 0000000..a2adece
Binary files /dev/null and b/Sampling_based_Planning/gif/Goal_biasd_RRT_2D.gif differ
diff --git a/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D.gif b/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D.gif
new file mode 100644
index 0000000..ca16b81
Binary files /dev/null and b/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D.gif differ
diff --git a/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D2.gif b/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D2.gif
new file mode 100644
index 0000000..cb25f47
Binary files /dev/null and b/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D2.gif differ
diff --git a/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D3.gif b/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D3.gif
new file mode 100644
index 0000000..019e42f
Binary files /dev/null and b/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D3.gif differ
diff --git a/Sampling_based_Planning/gif/RRT_2D.gif b/Sampling_based_Planning/gif/RRT_2D.gif
new file mode 100644
index 0000000..390faf2
Binary files /dev/null and b/Sampling_based_Planning/gif/RRT_2D.gif differ
diff --git a/Sampling-based Planning/gif/RRT_CONNECT_2D.gif b/Sampling_based_Planning/gif/RRT_CONNECT_2D.gif
similarity index 100%
rename from Sampling-based Planning/gif/RRT_CONNECT_2D.gif
rename to Sampling_based_Planning/gif/RRT_CONNECT_2D.gif
diff --git a/Sampling_based_Planning/gif/RRT_STAR2_2D.gif b/Sampling_based_Planning/gif/RRT_STAR2_2D.gif
new file mode 100644
index 0000000..e307d71
Binary files /dev/null and b/Sampling_based_Planning/gif/RRT_STAR2_2D.gif differ
diff --git a/Sampling_based_Planning/gif/RRT_STAR_2D.gif b/Sampling_based_Planning/gif/RRT_STAR_2D.gif
new file mode 100644
index 0000000..c4ddfcb
Binary files /dev/null and b/Sampling_based_Planning/gif/RRT_STAR_2D.gif differ
diff --git a/Sampling_based_Planning/gif/RRT_STAR_SMART_2D.gif b/Sampling_based_Planning/gif/RRT_STAR_SMART_2D.gif
new file mode 100644
index 0000000..083d611
Binary files /dev/null and b/Sampling_based_Planning/gif/RRT_STAR_SMART_2D.gif differ
diff --git a/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/env.cpython-37.pyc
similarity index 100%
rename from Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc
rename to Sampling_based_Planning/rrt_2D/__pycache__/env.cpython-37.pyc
diff --git a/Sampling_based_Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc
new file mode 100644
index 0000000..395452f
Binary files /dev/null and b/Sampling_based_Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc differ
diff --git a/Search-based Planning/__pycache__/queue.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/queue.cpython-37.pyc
similarity index 80%
rename from Search-based Planning/__pycache__/queue.cpython-37.pyc
rename to Sampling_based_Planning/rrt_2D/__pycache__/queue.cpython-37.pyc
index 82b863f..e4fd9fc 100644
Binary files a/Search-based Planning/__pycache__/queue.cpython-37.pyc and b/Sampling_based_Planning/rrt_2D/__pycache__/queue.cpython-37.pyc differ
diff --git a/Sampling_based_Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc
new file mode 100644
index 0000000..d22992d
Binary files /dev/null and b/Sampling_based_Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/utils.cpython-37.pyc
similarity index 73%
rename from Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc
rename to Sampling_based_Planning/rrt_2D/__pycache__/utils.cpython-37.pyc
index 1e50eef..137a52c 100644
Binary files a/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc and b/Sampling_based_Planning/rrt_2D/__pycache__/utils.cpython-37.pyc differ
diff --git a/Sampling_based_Planning/rrt_2D/adaptively_informed_trees.py b/Sampling_based_Planning/rrt_2D/adaptively_informed_trees.py
new file mode 100644
index 0000000..e69de29
diff --git a/Sampling_based_Planning/rrt_2D/advanced_batch_informed_trees.py b/Sampling_based_Planning/rrt_2D/advanced_batch_informed_trees.py
new file mode 100644
index 0000000..e69de29
diff --git a/Sampling_based_Planning/rrt_2D/batch_informed_trees.py b/Sampling_based_Planning/rrt_2D/batch_informed_trees.py
new file mode 100644
index 0000000..1996e15
--- /dev/null
+++ b/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()
diff --git a/Sampling_based_Planning/rrt_2D/dubins_rrt_star.py b/Sampling_based_Planning/rrt_2D/dubins_rrt_star.py
new file mode 100644
index 0000000..d5bc3ef
--- /dev/null
+++ b/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()
diff --git a/Sampling-based Planning/rrt_2D/dynamic_rrt.py b/Sampling_based_Planning/rrt_2D/dynamic_rrt.py
similarity index 98%
rename from Sampling-based Planning/rrt_2D/dynamic_rrt.py
rename to Sampling_based_Planning/rrt_2D/dynamic_rrt.py
index 1eef63d..e365971 100644
--- a/Sampling-based Planning/rrt_2D/dynamic_rrt.py
+++ b/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:
diff --git a/Sampling-based Planning/rrt_2D/env.py b/Sampling_based_Planning/rrt_2D/env.py
similarity index 100%
rename from Sampling-based Planning/rrt_2D/env.py
rename to Sampling_based_Planning/rrt_2D/env.py
diff --git a/Sampling-based Planning/rrt_2D/extended_rrt.py b/Sampling_based_Planning/rrt_2D/extended_rrt.py
similarity index 98%
rename from Sampling-based Planning/rrt_2D/extended_rrt.py
rename to Sampling_based_Planning/rrt_2D/extended_rrt.py
index 12c6a4e..746e365 100644
--- a/Sampling-based Planning/rrt_2D/extended_rrt.py
+++ b/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:
diff --git a/Sampling_based_Planning/rrt_2D/fast_marching_trees.py b/Sampling_based_Planning/rrt_2D/fast_marching_trees.py
new file mode 100644
index 0000000..efda0e5
--- /dev/null
+++ b/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()
diff --git a/Sampling_based_Planning/rrt_2D/informed_rrt_star.py b/Sampling_based_Planning/rrt_2D/informed_rrt_star.py
new file mode 100644
index 0000000..e34a88c
--- /dev/null
+++ b/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()
diff --git a/Sampling-based Planning/rrt_2D/plotting.py b/Sampling_based_Planning/rrt_2D/plotting.py
similarity index 89%
rename from Sampling-based Planning/rrt_2D/plotting.py
rename to Sampling_based_Planning/rrt_2D/plotting.py
index 37a3178..1df752b 100644
--- a/Sampling-based Planning/rrt_2D/plotting.py
+++ b/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()
diff --git a/Search-based Planning/Search_2D/queue.py b/Sampling_based_Planning/rrt_2D/queue.py
similarity index 100%
rename from Search-based Planning/Search_2D/queue.py
rename to Sampling_based_Planning/rrt_2D/queue.py
diff --git a/Sampling-based Planning/rrt_2D/rrt.py b/Sampling_based_Planning/rrt_2D/rrt.py
similarity index 92%
rename from Sampling-based Planning/rrt_2D/rrt.py
rename to Sampling_based_Planning/rrt_2D/rrt.py
index 80f42a9..1a1ecb2 100644
--- a/Sampling-based Planning/rrt_2D/rrt.py
+++ b/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!")
diff --git a/Sampling-based Planning/rrt_2D/rrt_connect.py b/Sampling_based_Planning/rrt_2D/rrt_connect.py
similarity index 97%
rename from Sampling-based Planning/rrt_2D/rrt_connect.py
rename to Sampling_based_Planning/rrt_2D/rrt_connect.py
index eb7c033..24a8a99 100644
--- a/Sampling-based Planning/rrt_2D/rrt_connect.py
+++ b/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:
diff --git a/Sampling_based_Planning/rrt_2D/rrt_sharp.py b/Sampling_based_Planning/rrt_2D/rrt_sharp.py
new file mode 100644
index 0000000..e69de29
diff --git a/Sampling-based Planning/rrt_2D/rrt_star.py b/Sampling_based_Planning/rrt_2D/rrt_star.py
similarity index 61%
rename from Sampling-based Planning/rrt_2D/rrt_star.py
rename to Sampling_based_Planning/rrt_2D/rrt_star.py
index ad066e3..bc84c84 100644
--- a/Sampling-based Planning/rrt_2D/rrt_star.py
+++ b/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,23 +45,65 @@ 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])
+ self.path = self.extract_path(self.vertex[index])
+
+ 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)
+
+ 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
@@ -72,79 +112,52 @@ class RrtStar:
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.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]))]
-
- def new_state(self, node_start, node_goal):
- dist, theta = self.get_distance_and_angle(node_start, node_goal)
-
- 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
+ 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 = []
+ @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]))]
- for i in neighbor_index:
- node_neighbor = self.vertex[i]
- cost.append(self.get_new_cost(node_neighbor, node_new))
+ @staticmethod
+ def cost(node_p):
+ node = node_p
+ cost = 0.0
- 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)
+ while node.parent:
+ cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
+ node = node.parent
- return node_new
+ return cost
- 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]
+ def update_cost(self, parent_node):
+ OPEN = queue.QueueFIFO()
+ OPEN.put(parent_node)
- 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 not OPEN.empty():
+ node = OPEN.get()
- return None
+ if len(node.child) == 0:
+ continue
- 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)
-
- if node_neighbor.cost > new_cost:
- self.vertex[i] = self.new_state(node_new, node_neighbor)
- self.propagate_cost_to_leaves(node_new)
-
- def get_new_cost(self, node_start, node_end):
- dist, _ = self.get_distance_and_angle(node_start, node_end)
- return node_start.cost + dist
-
- 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__':
diff --git a/Sampling_based_Planning/rrt_2D/rrt_star_smart.py b/Sampling_based_Planning/rrt_2D/rrt_star_smart.py
new file mode 100644
index 0000000..a847fc4
--- /dev/null
+++ b/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()
diff --git a/Sampling-based Planning/rrt_2D/utils.py b/Sampling_based_Planning/rrt_2D/utils.py
similarity index 96%
rename from Sampling-based Planning/rrt_2D/utils.py
rename to Sampling_based_Planning/rrt_2D/utils.py
index ca230a6..62dacb0 100644
--- a/Sampling-based Planning/rrt_2D/utils.py
+++ b/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:
diff --git a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc
similarity index 97%
rename from Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc
rename to Sampling_based_Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc
index 41188b3..39c891b 100644
Binary files a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc and b/Sampling_based_Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc
similarity index 96%
rename from Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc
rename to Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc
index b8da243..558f388 100644
Binary files a/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc and b/Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc
similarity index 100%
rename from Sampling-based Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc
rename to Sampling_based_Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc
diff --git a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc
similarity index 69%
rename from Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc
rename to Sampling_based_Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc
index 5a316fb..4b481cd 100644
Binary files a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc and b/Sampling_based_Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_3D/dynamic_rrt3D.py b/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py
similarity index 99%
rename from Sampling-based Planning/rrt_3D/dynamic_rrt3D.py
rename to Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py
index 8b24520..1d3324c 100644
--- a/Sampling-based Planning/rrt_3D/dynamic_rrt3D.py
+++ b/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
diff --git a/Sampling-based Planning/rrt_3D/env3D.py b/Sampling_based_Planning/rrt_3D/env3D.py
similarity index 100%
rename from Sampling-based Planning/rrt_3D/env3D.py
rename to Sampling_based_Planning/rrt_3D/env3D.py
diff --git a/Sampling-based Planning/rrt_3D/extend_rrt3D.py b/Sampling_based_Planning/rrt_3D/extend_rrt3D.py
similarity index 99%
rename from Sampling-based Planning/rrt_3D/extend_rrt3D.py
rename to Sampling_based_Planning/rrt_3D/extend_rrt3D.py
index 76cdcc4..58a6a0c 100644
--- a/Sampling-based Planning/rrt_3D/extend_rrt3D.py
+++ b/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
diff --git a/Sampling-based Planning/rrt_3D/plot_util3D.py b/Sampling_based_Planning/rrt_3D/plot_util3D.py
similarity index 100%
rename from Sampling-based Planning/rrt_3D/plot_util3D.py
rename to Sampling_based_Planning/rrt_3D/plot_util3D.py
diff --git a/Sampling-based Planning/rrt_3D/rrt3D.py b/Sampling_based_Planning/rrt_3D/rrt3D.py
similarity index 98%
rename from Sampling-based Planning/rrt_3D/rrt3D.py
rename to Sampling_based_Planning/rrt_3D/rrt3D.py
index e59bcbd..9c5444f 100644
--- a/Sampling-based Planning/rrt_3D/rrt3D.py
+++ b/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
diff --git a/Sampling-based Planning/rrt_3D/rrt_connect3D.py b/Sampling_based_Planning/rrt_3D/rrt_connect3D.py
similarity index 99%
rename from Sampling-based Planning/rrt_3D/rrt_connect3D.py
rename to Sampling_based_Planning/rrt_3D/rrt_connect3D.py
index c6264d2..1da6ede 100644
--- a/Sampling-based Planning/rrt_3D/rrt_connect3D.py
+++ b/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
diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling_based_Planning/rrt_3D/rrtstar3D.py
similarity index 99%
rename from Sampling-based Planning/rrt_3D/rrtstar3D.py
rename to Sampling_based_Planning/rrt_3D/rrtstar3D.py
index 25d63b6..ba5b5ed 100644
--- a/Sampling-based Planning/rrt_3D/rrtstar3D.py
+++ b/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
diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling_based_Planning/rrt_3D/utils3D.py
similarity index 96%
rename from Sampling-based Planning/rrt_3D/utils3D.py
rename to Sampling_based_Planning/rrt_3D/utils3D.py
index 9e485ca..02b3287 100644
--- a/Sampling-based Planning/rrt_3D/utils3D.py
+++ b/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
diff --git a/Search-based Planning/.idea/Search-based Planning.iml b/Search-based Planning/.idea/Search-based Planning.iml
deleted file mode 100644
index c444878..0000000
--- a/Search-based Planning/.idea/Search-based Planning.iml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Search-based Planning/.idea/dictionaries/Huiming_Zhou.xml b/Search-based Planning/.idea/dictionaries/Huiming_Zhou.xml
deleted file mode 100644
index f550db9..0000000
--- a/Search-based Planning/.idea/dictionaries/Huiming_Zhou.xml
+++ /dev/null
@@ -1,10 +0,0 @@
-
-
-
- astar
- dijk
- huiming
- zhou
-
-
-
\ No newline at end of file
diff --git a/Search-based Planning/.idea/inspectionProfiles/profiles_settings.xml b/Search-based Planning/.idea/inspectionProfiles/profiles_settings.xml
deleted file mode 100644
index 105ce2d..0000000
--- a/Search-based Planning/.idea/inspectionProfiles/profiles_settings.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Search-based Planning/.idea/misc.xml b/Search-based Planning/.idea/misc.xml
deleted file mode 100644
index a2e120d..0000000
--- a/Search-based Planning/.idea/misc.xml
+++ /dev/null
@@ -1,4 +0,0 @@
-
-
-
-
\ No newline at end of file
diff --git a/Search-based Planning/.idea/modules.xml b/Search-based Planning/.idea/modules.xml
deleted file mode 100644
index 1bc9d64..0000000
--- a/Search-based Planning/.idea/modules.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Search-based Planning/.idea/vcs.xml b/Search-based Planning/.idea/vcs.xml
deleted file mode 100644
index 6c0b863..0000000
--- a/Search-based Planning/.idea/vcs.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
deleted file mode 100644
index 669fe32..0000000
--- a/Search-based Planning/.idea/workspace.xml
+++ /dev/null
@@ -1,293 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- 1592347358698
-
-
- 1592347358698
-
-
- 1593715021929
-
-
-
- 1593715021929
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Search-based Planning/__pycache__/env.cpython-37.pyc b/Search-based Planning/__pycache__/env.cpython-37.pyc
deleted file mode 100644
index c88f4ce..0000000
Binary files a/Search-based Planning/__pycache__/env.cpython-37.pyc and /dev/null differ
diff --git a/Search-based Planning/__pycache__/plotting.cpython-35.pyc b/Search-based Planning/__pycache__/plotting.cpython-35.pyc
deleted file mode 100644
index c4a1fd1..0000000
Binary files a/Search-based Planning/__pycache__/plotting.cpython-35.pyc and /dev/null differ
diff --git a/Search-based Planning/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/__pycache__/plotting.cpython-37.pyc
deleted file mode 100644
index eb55214..0000000
Binary files a/Search-based Planning/__pycache__/plotting.cpython-37.pyc and /dev/null differ
diff --git a/Search-based Planning/__pycache__/queue.cpython-35.pyc b/Search-based Planning/__pycache__/queue.cpython-35.pyc
deleted file mode 100644
index e2155fd..0000000
Binary files a/Search-based Planning/__pycache__/queue.cpython-35.pyc and /dev/null differ
diff --git a/Search-based Planning/Search_2D/ARAstar.py b/Search_based_Planning/Search_2D/ARAstar.py
similarity index 96%
rename from Search-based Planning/Search_2D/ARAstar.py
rename to Search_based_Planning/Search_2D/ARAstar.py
index 58e3c12..b034665 100644
--- a/Search-based Planning/Search_2D/ARAstar.py
+++ b/Search_based_Planning/Search_2D/ARAstar.py
@@ -8,7 +8,7 @@ import sys
import math
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import plotting
from Search_2D import env
@@ -24,7 +24,7 @@ class AraStar:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.e = e # initial weight
- self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come
+ self.g = {self.s_start: 0, self.s_goal: float("inf")} # Cost to come
self.OPEN = {self.s_start: self.fvalue(self.s_start)} # priority queue / OPEN set
self.CLOSED = set() # CLOSED set
@@ -155,11 +155,11 @@ class AraStar:
def cost(self, s_start, s_goal):
"""
- Calculate cost for this motion
+ Calculate Cost for this motion
:param s_start: starting node
:param s_goal: end node
- :return: cost for this motion
- :note: cost function could be more complicate!
+ :return: Cost for this motion
+ :note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
diff --git a/Search-based Planning/Search_2D/Anytime_D_star.py b/Search_based_Planning/Search_2D/Anytime_D_star.py
similarity index 98%
rename from Search-based Planning/Search_2D/Anytime_D_star.py
rename to Search_based_Planning/Search_2D/Anytime_D_star.py
index c82b588..7d0b093 100644
--- a/Search-based Planning/Search_2D/Anytime_D_star.py
+++ b/Search_based_Planning/Search_2D/Anytime_D_star.py
@@ -9,7 +9,7 @@ import math
import matplotlib.pyplot as plt
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import plotting
from Search_2D import env
@@ -217,11 +217,11 @@ class ADStar:
def cost(self, s_start, s_goal):
"""
- Calculate cost for this motion
+ Calculate Cost for this motion
:param s_start: starting node
:param s_goal: end node
- :return: cost for this motion
- :note: cost function could be more complicate!
+ :return: Cost for this motion
+ :note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
diff --git a/Search-based Planning/Search_2D/Astar.py b/Search_based_Planning/Search_2D/Astar.py
similarity index 95%
rename from Search-based Planning/Search_2D/Astar.py
rename to Search_based_Planning/Search_2D/Astar.py
index 8d2361c..54b5919 100644
--- a/Search-based Planning/Search_2D/Astar.py
+++ b/Search_based_Planning/Search_2D/Astar.py
@@ -8,7 +8,7 @@ import sys
import math
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import queue
from Search_2D import plotting
@@ -25,7 +25,7 @@ class Astar:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
- self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come
+ self.g = {self.s_start: 0, self.s_goal: float("inf")} # Cost to come
self.OPEN = queue.QueuePrior() # priority queue / OPEN set
self.OPEN.put(self.s_start, self.fvalue(self.s_start))
self.CLOSED = [] # CLOSED set / VISITED order
@@ -48,7 +48,7 @@ class Astar:
new_cost = self.g[s] + self.cost(s, s_n)
if s_n not in self.g:
self.g[s_n] = float("inf")
- if new_cost < self.g[s_n]: # conditions for updating cost
+ if new_cost < self.g[s_n]: # conditions for updating Cost
self.g[s_n] = new_cost
self.PARENT[s_n] = s
self.OPEN.put(s_n, self.fvalue(s_n))
@@ -99,7 +99,7 @@ class Astar:
new_cost = g[s] + self.cost(s, s_n)
if s_n not in g:
g[s_n] = float("inf")
- if new_cost < g[s_n]: # conditions for updating cost
+ if new_cost < g[s_n]: # conditions for updating Cost
g[s_n] = new_cost
PARENT[s_n] = s
OPEN.put(s_n, g[s_n] + e * self.Heuristic(s_n))
@@ -122,11 +122,11 @@ class Astar:
def cost(self, s_start, s_goal):
"""
- Calculate cost for this motion
+ Calculate Cost for this motion
:param s_start: starting node
:param s_goal: end node
- :return: cost for this motion
- :note: cost function could be more complicate!
+ :return: Cost for this motion
+ :note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
@@ -153,7 +153,7 @@ class Astar:
def fvalue(self, x):
"""
- f = g + h. (g: cost to come, h: heuristic function)
+ f = g + h. (g: Cost to come, h: heuristic function)
:param x: current state
:return: f
"""
diff --git a/Search-based Planning/Search_2D/Best_First.py b/Search_based_Planning/Search_2D/Best_First.py
similarity index 98%
rename from Search-based Planning/Search_2D/Best_First.py
rename to Search_based_Planning/Search_2D/Best_First.py
index 326e73b..8c6b4c3 100644
--- a/Search-based Planning/Search_2D/Best_First.py
+++ b/Search_based_Planning/Search_2D/Best_First.py
@@ -8,7 +8,7 @@ import sys
import math
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import queue
from Search_2D import plotting
diff --git a/Search-based Planning/Search_2D/Bidirectional_a_star.py b/Search_based_Planning/Search_2D/Bidirectional_a_star.py
similarity index 95%
rename from Search-based Planning/Search_2D/Bidirectional_a_star.py
rename to Search_based_Planning/Search_2D/Bidirectional_a_star.py
index 4f01dfd..692dc5e 100644
--- a/Search-based Planning/Search_2D/Bidirectional_a_star.py
+++ b/Search_based_Planning/Search_2D/Bidirectional_a_star.py
@@ -8,7 +8,7 @@ import sys
import math
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import queue
from Search_2D import plotting
@@ -25,8 +25,8 @@ class BidirectionalAstar:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
- self.g_fore = {self.s_start: 0, self.s_goal: float("inf")} # cost to come: from s_start
- self.g_back = {self.s_goal: 0, self.s_start: float("inf")} # cost to come: form s_goal
+ self.g_fore = {self.s_start: 0, self.s_goal: float("inf")} # Cost to come: from x_init
+ self.g_back = {self.s_goal: 0, self.s_start: float("inf")} # Cost to come: form x_goal
self.OPEN_fore = queue.QueuePrior() # OPEN set for foreward searching
self.OPEN_fore.put(self.s_start,
@@ -142,11 +142,11 @@ class BidirectionalAstar:
def cost(self, s_start, s_goal):
"""
- Calculate cost for this motion
+ Calculate Cost for this motion
:param s_start: starting node
:param s_goal: end node
- :return: cost for this motion
- :note: cost function could be more complicate!
+ :return: Cost for this motion
+ :note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
diff --git a/Search-based Planning/Search_2D/D_star.py b/Search_based_Planning/Search_2D/D_star.py
similarity index 97%
rename from Search-based Planning/Search_2D/D_star.py
rename to Search_based_Planning/Search_2D/D_star.py
index 792620a..45bf9b7 100644
--- a/Search-based Planning/Search_2D/D_star.py
+++ b/Search_based_Planning/Search_2D/D_star.py
@@ -9,7 +9,7 @@ import math
import matplotlib.pyplot as plt
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import plotting
from Search_2D import env
@@ -177,11 +177,11 @@ class Dstar:
def cost(self, s_start, s_goal):
"""
- Calculate cost for this motion
+ Calculate Cost for this motion
:param s_start: starting node
:param s_goal: end node
- :return: cost for this motion
- :note: cost function could be more complicate!
+ :return: Cost for this motion
+ :note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
diff --git a/Search-based Planning/Search_2D/D_star_Lite.py b/Search_based_Planning/Search_2D/D_star_Lite.py
similarity index 97%
rename from Search-based Planning/Search_2D/D_star_Lite.py
rename to Search_based_Planning/Search_2D/D_star_Lite.py
index ea10baf..ed0fe04 100644
--- a/Search-based Planning/Search_2D/D_star_Lite.py
+++ b/Search_based_Planning/Search_2D/D_star_Lite.py
@@ -9,7 +9,7 @@ import math
import matplotlib.pyplot as plt
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import plotting
from Search_2D import env
@@ -150,11 +150,11 @@ class DStar:
def cost(self, s_start, s_goal):
"""
- Calculate cost for this motion
+ Calculate Cost for this motion
:param s_start: starting node
:param s_goal: end node
- :return: cost for this motion
- :note: cost function could be more complicate!
+ :return: Cost for this motion
+ :note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
diff --git a/Search-based Planning/Search_2D/Dijkstra.py b/Search_based_Planning/Search_2D/Dijkstra.py
similarity index 92%
rename from Search-based Planning/Search_2D/Dijkstra.py
rename to Search_based_Planning/Search_2D/Dijkstra.py
index 1d98611..0b06239 100644
--- a/Search-based Planning/Search_2D/Dijkstra.py
+++ b/Search_based_Planning/Search_2D/Dijkstra.py
@@ -8,11 +8,9 @@ import sys
import math
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
-from Search_2D import queue
-from Search_2D import plotting
-from Search_2D import env
+from Search_based_Planning.Search_2D import queue, plotting, env
class Dijkstra:
@@ -25,7 +23,7 @@ class Dijkstra:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
- self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come
+ self.g = {self.s_start: 0, self.s_goal: float("inf")} # Cost to come
self.OPEN = queue.QueuePrior() # priority queue / OPEN set
self.OPEN.put(self.s_start, 0)
self.CLOSED = [] # closed set & visited
@@ -89,11 +87,11 @@ class Dijkstra:
def cost(self, s_start, s_goal):
"""
- Calculate cost for this motion
+ Calculate Cost for this motion
:param s_start: starting node
:param s_goal: end node
- :return: cost for this motion
- :note: cost function could be more complicate!
+ :return: Cost for this motion
+ :note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search_based_Planning/Search_2D/LPAstar.py
similarity index 97%
rename from Search-based Planning/Search_2D/LPAstar.py
rename to Search_based_Planning/Search_2D/LPAstar.py
index 70e816a..cb23701 100644
--- a/Search-based Planning/Search_2D/LPAstar.py
+++ b/Search_based_Planning/Search_2D/LPAstar.py
@@ -9,7 +9,7 @@ import math
import matplotlib.pyplot as plt
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import plotting
from Search_2D import env
@@ -149,11 +149,11 @@ class LpaStar:
def cost(self, s_start, s_goal):
"""
- Calculate cost for this motion
+ Calculate Cost for this motion
:param s_start: starting node
:param s_goal: end node
- :return: cost for this motion
- :note: cost function could be more complicate!
+ :return: Cost for this motion
+ :note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
diff --git a/Search-based Planning/Search_2D/LRTAstar.py b/Search_based_Planning/Search_2D/LRTAstar.py
similarity index 96%
rename from Search-based Planning/Search_2D/LRTAstar.py
rename to Search_based_Planning/Search_2D/LRTAstar.py
index 7363f0e..3d7bc23 100644
--- a/Search-based Planning/Search_2D/LRTAstar.py
+++ b/Search_based_Planning/Search_2D/LRTAstar.py
@@ -9,7 +9,7 @@ import copy
import math
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import queue
from Search_2D import plotting
@@ -50,7 +50,7 @@ class LrtAstarN:
for x in h_value:
self.h_table[x] = h_value[x]
- s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # s_start -> expected node in OPEN set
+ s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # x_init -> expected node in OPEN set
self.path.append(path_k)
def extract_path_in_CLOSE(self, s_start, h_value):
@@ -95,7 +95,7 @@ class LrtAstarN:
OPEN = queue.QueuePrior() # OPEN set
OPEN.put(x_start, self.h(x_start))
CLOSED = [] # CLOSED set
- g_table = {x_start: 0, self.s_goal: float("inf")} # cost to come
+ g_table = {x_start: 0, self.s_goal: float("inf")} # Cost to come
PARENT = {x_start: x_start} # relations
count = 0 # counter
@@ -113,7 +113,7 @@ class LrtAstarN:
new_cost = g_table[s] + self.cost(s, s_n)
if s_n not in g_table:
g_table[s_n] = float("inf")
- if new_cost < g_table[s_n]: # conditions for updating cost
+ if new_cost < g_table[s_n]: # conditions for updating Cost
g_table[s_n] = new_cost
PARENT[s_n] = s
OPEN.put(s_n, g_table[s_n] + self.h_table[s_n])
@@ -177,11 +177,11 @@ class LrtAstarN:
def cost(self, s_start, s_goal):
"""
- Calculate cost for this motion
+ Calculate Cost for this motion
:param s_start: starting node
:param s_goal: end node
- :return: cost for this motion
- :note: cost function could be more complicate!
+ :return: Cost for this motion
+ :note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
diff --git a/Search-based Planning/Search_2D/PotentialField.py b/Search_based_Planning/Search_2D/PotentialField.py
similarity index 98%
rename from Search-based Planning/Search_2D/PotentialField.py
rename to Search_based_Planning/Search_2D/PotentialField.py
index 44d0c85..dfa82e7 100644
--- a/Search-based Planning/Search_2D/PotentialField.py
+++ b/Search_based_Planning/Search_2D/PotentialField.py
@@ -10,7 +10,7 @@ import matplotlib.pyplot as plt
from collections import deque
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import plotting
from Search_2D import env
diff --git a/Search-based Planning/Search_2D/RTAAstar.py b/Search_based_Planning/Search_2D/RTAAstar.py
similarity index 97%
rename from Search-based Planning/Search_2D/RTAAstar.py
rename to Search_based_Planning/Search_2D/RTAAstar.py
index 3575eac..8c59d91 100644
--- a/Search-based Planning/Search_2D/RTAAstar.py
+++ b/Search_based_Planning/Search_2D/RTAAstar.py
@@ -9,7 +9,7 @@ import copy
import math
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import queue
from Search_2D import plotting
@@ -90,7 +90,7 @@ class RtaAstar:
OPEN = queue.QueuePrior() # OPEN set
OPEN.put(x_start, self.h_table[x_start])
CLOSED = [] # CLOSED set
- g_table = {x_start: 0, self.s_goal: float("inf")} # cost to come
+ g_table = {x_start: 0, self.s_goal: float("inf")} # Cost to come
PARENT = {x_start: x_start} # relations
count = 0 # counter
@@ -108,7 +108,7 @@ class RtaAstar:
new_cost = g_table[s] + self.cost(s, s_n)
if s_n not in g_table:
g_table[s_n] = float("inf")
- if new_cost < g_table[s_n]: # conditions for updating cost
+ if new_cost < g_table[s_n]: # conditions for updating Cost
g_table[s_n] = new_cost
PARENT[s_n] = s
OPEN.put(s_n, g_table[s_n] + self.h_table[s_n])
@@ -186,11 +186,11 @@ class RtaAstar:
def cost(self, s_start, s_goal):
"""
- Calculate cost for this motion
+ Calculate Cost for this motion
:param s_start: starting node
:param s_goal: end node
- :return: cost for this motion
- :note: cost function could be more complicate!
+ :return: Cost for this motion
+ :note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
diff --git a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc b/Search_based_Planning/Search_2D/__pycache__/env.cpython-37.pyc
similarity index 100%
rename from Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc
rename to Search_based_Planning/Search_2D/__pycache__/env.cpython-37.pyc
diff --git a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc b/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-37.pyc
similarity index 74%
rename from Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc
rename to Search_based_Planning/Search_2D/__pycache__/plotting.cpython-37.pyc
index 1c78399..2fdee55 100644
Binary files a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc and b/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-37.pyc differ
diff --git a/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc b/Search_based_Planning/Search_2D/__pycache__/queue.cpython-37.pyc
similarity index 100%
rename from Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc
rename to Search_based_Planning/Search_2D/__pycache__/queue.cpython-37.pyc
diff --git a/Search-based Planning/Search_2D/bfs.py b/Search_based_Planning/Search_2D/bfs.py
similarity index 98%
rename from Search-based Planning/Search_2D/bfs.py
rename to Search_based_Planning/Search_2D/bfs.py
index efadfbb..dffa661 100644
--- a/Search-based Planning/Search_2D/bfs.py
+++ b/Search_based_Planning/Search_2D/bfs.py
@@ -7,7 +7,7 @@ import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import queue
from Search_2D import plotting
diff --git a/Search-based Planning/Search_2D/dfs.py b/Search_based_Planning/Search_2D/dfs.py
similarity index 98%
rename from Search-based Planning/Search_2D/dfs.py
rename to Search_based_Planning/Search_2D/dfs.py
index 6e45d72..820680b 100644
--- a/Search-based Planning/Search_2D/dfs.py
+++ b/Search_based_Planning/Search_2D/dfs.py
@@ -7,7 +7,7 @@ import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import queue
from Search_2D import plotting
diff --git a/Search-based Planning/Search_2D/env.py b/Search_based_Planning/Search_2D/env.py
similarity index 100%
rename from Search-based Planning/Search_2D/env.py
rename to Search_based_Planning/Search_2D/env.py
diff --git a/Search-based Planning/Search_2D/plotting.py b/Search_based_Planning/Search_2D/plotting.py
similarity index 98%
rename from Search-based Planning/Search_2D/plotting.py
rename to Search_based_Planning/Search_2D/plotting.py
index c818a64..f952876 100644
--- a/Search-based Planning/Search_2D/plotting.py
+++ b/Search_based_Planning/Search_2D/plotting.py
@@ -8,7 +8,7 @@ import sys
import matplotlib.pyplot as plt
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
- "/../../Search-based Planning/")
+ "/../../Search_based_Planning/")
from Search_2D import env
diff --git a/Search_based_Planning/Search_2D/queue.py b/Search_based_Planning/Search_2D/queue.py
new file mode 100644
index 0000000..8f481ae
--- /dev/null
+++ b/Search_based_Planning/Search_2D/queue.py
@@ -0,0 +1,62 @@
+import collections
+import heapq
+
+
+class QueueFIFO:
+ """
+ Class: QueueFIFO
+ Description: QueueFIFO is designed for First-in-First-out rule.
+ """
+
+ def __init__(self):
+ self.queue = collections.deque()
+
+ def empty(self):
+ return len(self.queue) == 0
+
+ def put(self, node):
+ self.queue.append(node) # enter from back
+
+ def get(self):
+ return self.queue.popleft() # leave from front
+
+
+class QueueLIFO:
+ """
+ Class: QueueLIFO
+ Description: QueueLIFO is designed for Last-in-First-out rule.
+ """
+
+ def __init__(self):
+ self.queue = collections.deque()
+
+ def empty(self):
+ return len(self.queue) == 0
+
+ def put(self, node):
+ self.queue.append(node) # enter from back
+
+ def get(self):
+ return self.queue.pop() # leave from back
+
+
+class QueuePrior:
+ """
+ Class: QueuePrior
+ Description: QueuePrior reorders elements using value [priority]
+ """
+
+ def __init__(self):
+ self.queue = []
+
+ def empty(self):
+ return len(self.queue) == 0
+
+ def put(self, item, priority):
+ heapq.heappush(self.queue, (priority, item)) # reorder x using priority
+
+ def get(self):
+ return heapq.heappop(self.queue)[1] # pop out the smallest item
+
+ def enumerate(self):
+ return self.queue
diff --git a/Search-based Planning/Search_3D/Anytime_Dstar3D.py b/Search_based_Planning/Search_3D/Anytime_Dstar3D.py
similarity index 93%
rename from Search-based Planning/Search_3D/Anytime_Dstar3D.py
rename to Search_based_Planning/Search_3D/Anytime_Dstar3D.py
index 43c0307..bc7fa62 100644
--- a/Search-based Planning/Search_3D/Anytime_Dstar3D.py
+++ b/Search_based_Planning/Search_3D/Anytime_Dstar3D.py
@@ -7,7 +7,7 @@ import os
import sys
from collections import defaultdict
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/")
from Search_3D.env3D import env
from Search_3D.utils3D import getDist, heuristic_fun, getNearest, isinbound, \
cost, children, StateSpace
@@ -41,7 +41,7 @@ class Anytime_Dstar(object):
# init children set:
self.CHILDREN = {}
- # init cost set
+ # init Cost set
self.COST = defaultdict(lambda: defaultdict(dict))
# for visualization
@@ -88,7 +88,7 @@ class Anytime_Dstar(object):
return self.rhs[xi]
def updatecost(self, range_changed=None, new=None, old=None, mode=False):
- # scan graph for changed cost, if cost is changed update it
+ # scan graph for changed Cost, if Cost is changed update it
CHANGED = set()
for xi in self.CLOSED:
if isinbound(old, xi, mode) or isinbound(new, xi, mode):
@@ -100,8 +100,8 @@ class Anytime_Dstar(object):
return CHANGED
# def updateGraphCost(self, range_changed=None, new=None, old=None, mode=False):
- # # TODO scan graph for changed cost, if cost is changed update it
- # # make the graph cost via vectorization
+ # # TODO scan graph for changed Cost, if Cost is changed update it
+ # # make the graph Cost via vectorization
# CHANGED = set()
# Allnodes = np.array(list(self.CLOSED))
# isChanged = isinbound(old, Allnodes, mode = mode, isarray = True) & \
@@ -112,7 +112,7 @@ class Anytime_Dstar(object):
# CHANGED.add(xi)
# self.CHILDREN[xi] = set(children(self, xi))
# for xj in self.CHILDREN:
- # self.COST[xi][xj] = cost(self, xi, xj)
+ # self.COST[xi][xj] = Cost(self, xi, xj)
# --------------main functions for Anytime D star
@@ -177,7 +177,7 @@ class Anytime_Dstar(object):
# islargelychanged = True
self.Path = []
- # update cost with changed environment
+ # update Cost with changed environment
if ischanged:
# CHANGED = self.updatecost(True, new2, old2, mode='obb')
CHANGED = self.updatecost(True, new2, old2)
@@ -207,10 +207,10 @@ class Anytime_Dstar(object):
def path(self, s_start=None):
'''After ComputeShortestPath()
- returns, one can then follow a shortest path from s_start to
- s_goal by always moving from the current vertex s, starting
- at s_start. , to any successor s' that minimizes c(s,s') + g(s')
- until s_goal is reached (ties can be broken arbitrarily).'''
+ returns, one can then follow a shortest path from x_init to
+ x_goal by always moving from the current vertex s, starting
+ at x_init. , to any successor s' that minimizes cBest(s,s') + g(s')
+ until x_goal is reached (ties can be broken arbitrarily).'''
path = []
s_goal = self.xt
s = self.x0
diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search_based_Planning/Search_3D/Astar3D.py
similarity index 99%
rename from Search-based Planning/Search_3D/Astar3D.py
rename to Search_based_Planning/Search_3D/Astar3D.py
index 961e2ba..4b7ad68 100644
--- a/Search-based Planning/Search_3D/Astar3D.py
+++ b/Search_based_Planning/Search_3D/Astar3D.py
@@ -10,7 +10,7 @@ import matplotlib.pyplot as plt
import os
import sys
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/")
from Search_3D.env3D import env
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \
cost, children, StateSpace, heuristic_fun
diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search_based_Planning/Search_3D/Dstar3D.py
similarity index 98%
rename from Search-based Planning/Search_3D/Dstar3D.py
rename to Search_based_Planning/Search_3D/Dstar3D.py
index 09d4cea..9332d4e 100644
--- a/Search-based Planning/Search_3D/Dstar3D.py
+++ b/Search_based_Planning/Search_3D/Dstar3D.py
@@ -5,7 +5,7 @@ import os
import sys
from collections import defaultdict
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/")
from Search_3D.env3D import env
from Search_3D import Astar3D
from Search_3D.utils3D import StateSpace, getDist, getNearest, getRay, isinbound, isinball, isCollide, children, cost, \
@@ -175,7 +175,7 @@ class D_star(object):
sparent = self.b[self.x0]
else:
sparent = self.b[s]
- # if there is a change of cost, or a collision.
+ # if there is a change of Cost, or a collision.
if cost(self, s, sparent) == np.inf:
self.modify(s)
continue
diff --git a/Search-based Planning/Search_3D/DstarLite3D.py b/Search_based_Planning/Search_3D/DstarLite3D.py
similarity index 93%
rename from Search-based Planning/Search_3D/DstarLite3D.py
rename to Search_based_Planning/Search_3D/DstarLite3D.py
index d328f64..316de8f 100644
--- a/Search-based Planning/Search_3D/DstarLite3D.py
+++ b/Search_based_Planning/Search_3D/DstarLite3D.py
@@ -5,7 +5,7 @@ import os
import sys
from collections import defaultdict
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/")
from Search_3D.env3D import env
from Search_3D.utils3D import getDist, heuristic_fun, getNearest, isinbound, \
cost, children, StateSpace
@@ -41,7 +41,7 @@ class D_star_Lite(object):
# init children set:
self.CHILDREN = {}
- # init cost set
+ # init Cost set
self.COST = defaultdict(lambda: defaultdict(dict))
# for visualization
@@ -51,7 +51,7 @@ class D_star_Lite(object):
self.done = False
def updatecost(self, range_changed=None, new=None, old=None, mode=False):
- # scan graph for changed cost, if cost is changed update it
+ # scan graph for changed Cost, if Cost is changed update it
CHANGED = set()
for xi in self.CLOSED:
if isinbound(old, xi, mode) or isinbound(new, xi, mode):
@@ -100,7 +100,7 @@ class D_star_Lite(object):
def UpdateVertex(self, u):
# if still in the hunt
- if not getDist(self.xt, u) <= self.env.resolution: # originally: u != s_goal
+ if not getDist(self.xt, u) <= self.env.resolution: # originally: u != x_goal
if u in self.CHILDREN and len(self.CHILDREN[u]) == 0:
self.rhs[u] = np.inf
else:
@@ -147,7 +147,7 @@ class D_star_Lite(object):
ischanged = False
self.V = set()
while getDist(self.x0, self.xt) > 2*self.env.resolution:
- #---------------------------------- at specific times, the environment is changed and cost is updated
+ #---------------------------------- at specific times, the environment is changed and Cost is updated
if t % 2 == 0:
new0,old0 = self.env.move_block(a=[-0.1, 0, -0.2], s=0.5, block_to_move=1, mode='translation')
new1,old1 = self.env.move_block(a=[0, 0, -0.2], s=0.5, block_to_move=0, mode='translation')
@@ -163,9 +163,9 @@ class D_star_Lite(object):
self.x0 = children_new[np.argmin([self.getcost(self.x0,s_p) + self.getg(s_p) for s_p in children_new])]
# TODO add the moving robot position codes
self.env.start = self.x0
- # ---------------------------------- if any cost changed, update km, reset slast,
+ # ---------------------------------- if any Cost changed, update km, reset slast,
# for all directed edgees (u,v) with chaged edge costs,
- # update the edge cost c(u,v) and update vertex u. then replan
+ # update the edge Cost cBest(u,v) and update vertex u. then replan
if ischanged:
self.km += heuristic_fun(self, self.x0, s_last)
s_last = self.x0
@@ -186,10 +186,10 @@ class D_star_Lite(object):
def path(self, s_start=None):
'''After ComputeShortestPath()
- returns, one can then follow a shortest path from s_start to
- s_goal by always moving from the current vertex s, starting
- at s_start. , to any successor s' that minimizes c(s,s') + g(s')
- until s_goal is reached (ties can be broken arbitrarily).'''
+ returns, one can then follow a shortest path from x_init to
+ x_goal by always moving from the current vertex s, starting
+ at x_init. , to any successor s' that minimizes cBest(s,s') + g(s')
+ until x_goal is reached (ties can be broken arbitrarily).'''
path = []
s_goal = self.xt
if not s_start:
diff --git a/Search-based Planning/Search_3D/LP_Astar3D.py b/Search_based_Planning/Search_3D/LP_Astar3D.py
similarity index 97%
rename from Search-based Planning/Search_3D/LP_Astar3D.py
rename to Search_based_Planning/Search_3D/LP_Astar3D.py
index a4df422..7a5ca1d 100644
--- a/Search-based Planning/Search_3D/LP_Astar3D.py
+++ b/Search_based_Planning/Search_3D/LP_Astar3D.py
@@ -4,7 +4,7 @@ import matplotlib.pyplot as plt
import os
import sys
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/")
from Search_3D.env3D import env
from Search_3D import Astar3D
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isinbound, isinball, \
@@ -48,7 +48,7 @@ class Lifelong_Astar(object):
self.CHILDREN = {}
self.getCHILDRENset()
- # initialize cost list
+ # initialize Cost list
self.COST = {}
_ = self.costset()
@@ -58,7 +58,7 @@ class Lifelong_Astar(object):
children = self.CHILDREN[xi]
toUpdate = [self.cost(xj,xi) for xj in children]
if xi in self.COST:
- # if the old cost not equal to new cost
+ # if the old Cost not equal to new Cost
diff = np.not_equal(self.COST[xi],toUpdate)
cd = np.array(children)[diff]
for i in cd:
@@ -126,7 +126,7 @@ class Lifelong_Astar(object):
j = x
nei = self.CHILDREN[x]
gset = [self.g[xi] for xi in nei]
- # collision check and make g cost inf
+ # collision check and make g Cost inf
for i in range(len(nei)):
if self.isCollide(nei[i],j)[0]:
gset[i] = np.inf
diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search_based_Planning/Search_3D/LRT_Astar3D.py
similarity index 96%
rename from Search-based Planning/Search_3D/LRT_Astar3D.py
rename to Search_based_Planning/Search_3D/LRT_Astar3D.py
index dac6c0e..89acf9a 100644
--- a/Search-based Planning/Search_3D/LRT_Astar3D.py
+++ b/Search_based_Planning/Search_3D/LRT_Astar3D.py
@@ -10,7 +10,7 @@ import matplotlib.pyplot as plt
import os
import sys
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/")
from Search_3D.env3D import env
from Search_3D import Astar3D
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \
@@ -36,7 +36,7 @@ class LRT_A_star2:
# update h values if they are smaller
Children = children(self.Astar,xi)
minfval = min([cost(self.Astar,xi, xj, settings=0) + self.Astar.h[xj] for xj in Children])
- # h(s) = h(s') if h(s) > c(s,s') + h(s')
+ # h(s) = h(s') if h(s) > cBest(s,s') + h(s')
if self.Astar.h[xi] >= minfval:
self.Astar.h[xi] = minfval
hvals.append(self.Astar.h[xi])
diff --git a/Search-based Planning/Search_3D/RTA_Astar3D.py b/Search_based_Planning/Search_3D/RTA_Astar3D.py
similarity index 98%
rename from Search-based Planning/Search_3D/RTA_Astar3D.py
rename to Search_based_Planning/Search_3D/RTA_Astar3D.py
index 5e93ebf..26548b0 100644
--- a/Search-based Planning/Search_3D/RTA_Astar3D.py
+++ b/Search_based_Planning/Search_3D/RTA_Astar3D.py
@@ -10,7 +10,7 @@ import matplotlib.pyplot as plt
import os
import sys
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/")
from Search_3D.env3D import env
from Search_3D import Astar3D
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \
diff --git a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc
similarity index 100%
rename from Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc
rename to Search_based_Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc
diff --git a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/env3D.cpython-37.pyc
similarity index 100%
rename from Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc
rename to Search_based_Planning/Search_3D/__pycache__/env3D.cpython-37.pyc
diff --git a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc
similarity index 100%
rename from Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc
rename to Search_based_Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc
diff --git a/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/queue.cpython-37.pyc
similarity index 100%
rename from Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc
rename to Search_based_Planning/Search_3D/__pycache__/queue.cpython-37.pyc
diff --git a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc
similarity index 100%
rename from Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc
rename to Search_based_Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc
diff --git a/Search-based Planning/Search_3D/bidirectional_Astar3D.py b/Search_based_Planning/Search_3D/bidirectional_Astar3D.py
similarity index 99%
rename from Search-based Planning/Search_3D/bidirectional_Astar3D.py
rename to Search_based_Planning/Search_3D/bidirectional_Astar3D.py
index da41482..2f1c48d 100644
--- a/Search-based Planning/Search_3D/bidirectional_Astar3D.py
+++ b/Search_based_Planning/Search_3D/bidirectional_Astar3D.py
@@ -11,7 +11,7 @@ from collections import defaultdict
import os
import sys
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/")
from Search_3D.env3D import env
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost, children, heuristic_fun
from Search_3D.plot_util3D import visualization
diff --git a/Search-based Planning/Search_3D/env3D.py b/Search_based_Planning/Search_3D/env3D.py
similarity index 97%
rename from Search-based Planning/Search_3D/env3D.py
rename to Search_based_Planning/Search_3D/env3D.py
index 1d1a59a..c8805ab 100644
--- a/Search-based Planning/Search_3D/env3D.py
+++ b/Search_based_Planning/Search_3D/env3D.py
@@ -1,179 +1,179 @@
-# this is the three dimensional configuration space for rrt
-# !/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-@author: yue qi
-"""
-import numpy as np
-
-
-# from utils3D import OBB2AABB
-
-def R_matrix(z_angle, y_angle, x_angle):
- # x angle: row; y angle: pitch; z angle: yaw
- # generate rotation matrix in SO3
- # RzRyRx = R, ZYX intrinsic rotation
- # also (r1,r2,r3) in R3*3 in {W} frame
- # used in obb.O
- # [[R p]
- # [0T 1]] gives transformation from body to world
- return np.array(
- [[np.cos(z_angle), -np.sin(z_angle), 0.0], [np.sin(z_angle), np.cos(z_angle), 0.0], [0.0, 0.0, 1.0]]) @ \
- np.array(
- [[np.cos(y_angle), 0.0, np.sin(y_angle)], [0.0, 1.0, 0.0], [-np.sin(y_angle), 0.0, np.cos(y_angle)]]) @ \
- np.array(
- [[1.0, 0.0, 0.0], [0.0, np.cos(x_angle), -np.sin(x_angle)], [0.0, np.sin(x_angle), np.cos(x_angle)]])
-
-
-def getblocks():
- # AABBs
- block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00],
- [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00],
- # [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00],
- # [1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00],
- # [6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00],
- [1.21e+01, 0.00e+00, 0.00e+00, 1.29e+01, 5.00e+00, 3.90e+00],
- [1.81e+01, 0.00e+00, 0.00e+00, 1.89e+01, 5.00e+00, 3.90e+00]]
- Obstacles = []
- for i in block:
- i = np.array(i)
- Obstacles.append([j for j in i])
- return np.array(Obstacles)
-
-
-def getAABB(blocks):
- # used for Pyrr package for detecting collision
- AABB = []
- for i in blocks:
- AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger
- return AABB
-
-
-class aabb(object):
- # make AABB out of blocks,
- # P: center point
- # E: extents
- # O: Rotation matrix in SO(3), in {w}
- def __init__(self, AABB):
- self.P = [(AABB[3] + AABB[0]) / 2, (AABB[4] + AABB[1]) / 2, (AABB[5] + AABB[2]) / 2] # center point
- self.E = [(AABB[3] - AABB[0]) / 2, (AABB[4] - AABB[1]) / 2, (AABB[5] - AABB[2]) / 2] # extents
- self.O = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
-
-
-class obb(object):
- # P: center point
- # E: extents
- # O: Rotation matrix in SO(3), in {w}
- def __init__(self, P, E, O):
- self.P = P
- self.E = E
- self.O = O
- self.T = np.vstack([np.column_stack([self.O.T, -self.O.T @ self.P]), [0, 0, 0, 1]])
-
-
-def getAABB2(blocks):
- # used in lineAABB
- AABB = []
- for i in blocks:
- AABB.append(aabb(i))
- return AABB
-
-
-def getballs():
- spheres = [[16, 2.5, 4, 2], [10, 2.5, 1, 1]]
- Obstacles = []
- for i in spheres:
- Obstacles.append([j for j in i])
- return np.array(Obstacles)
-
-
-def add_block(block=[1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]):
- return block
-
-
-class env():
- def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1):
- self.resolution = resolution
- self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax])
- self.blocks = getblocks()
- self.AABB = getAABB2(self.blocks)
- self.AABB_pyrr = getAABB(self.blocks)
- self.balls = getballs()
- self.OBB = np.array([obb([2.6, 2.5, 1], [0.2, 2, 1], R_matrix(0, 0, 45))])
- # self.OBB = np.squeeze(np.vstack([self.OBB,OBB2AABB(self.OBB[0])]))
- # print(self.OBB)
- # self.OBB = []
- self.start = np.array([0.5, 2.5, 5.5])
- self.goal = np.array([19.0, 2.5, 5.5])
- self.t = 0 # time
-
- def New_block(self):
- newblock = add_block()
- self.blocks = np.vstack([self.blocks, newblock])
- self.AABB = getAABB2(self.blocks)
- self.AABB_pyrr = getAABB(self.blocks)
-
- def move_start(self, x):
- self.start = x
-
- def move_block(self, a=[0, 0, 0], s=0, v=[0.1, 0, 0], theta=[0, 0, 0], block_to_move=0, obb_to_move=0,
- mode='uniform'):
- # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time,
- # R is an orthorgonal transform in R3*3, is the rotation matrix
- # (x',t') = (x + tv, t) is uniform transformation
- if mode == 'uniform':
- ori = np.array(self.blocks[block_to_move])
- self.blocks[block_to_move] = \
- np.array([ori[0] + self.t * v[0],
- ori[1] + self.t * v[1],
- ori[2] + self.t * v[2],
- ori[3] + self.t * v[0],
- ori[4] + self.t * v[1],
- ori[5] + self.t * v[2]])
-
- self.AABB[block_to_move].P = \
- [self.AABB[block_to_move].P[0] + self.t * v[0],
- self.AABB[block_to_move].P[1] + self.t * v[1],
- self.AABB[block_to_move].P[2] + self.t * v[2]]
- # return a range of block that the block might moved
- a = self.blocks[block_to_move]
- # return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \
- # a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]). \
- # np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \
- # ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
- return a, ori
- # (x',t') = (x + a, t + s) is a translation
- if mode == 'translation':
- ori = np.array(self.blocks[block_to_move])
- self.blocks[block_to_move] = \
- np.array([ori[0] + a[0],
- ori[1] + a[1],
- ori[2] + a[2],
- ori[3] + a[0],
- ori[4] + a[1],
- ori[5] + a[2]])
-
- self.AABB[block_to_move].P = \
- [self.AABB[block_to_move].P[0] + a[0],
- self.AABB[block_to_move].P[1] + a[1],
- self.AABB[block_to_move].P[2] + a[2]]
- self.t += s
- # return a range of block that the block might moved
- a = self.blocks[block_to_move]
- return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution,
- a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \
- np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution,
- ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
- # return a,ori
- # (x',t') = (Rx, t)
- if mode == 'rotation': # this makes an OBB rotate
- ori = [self.OBB[obb_to_move]]
- self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0], y_angle=theta[1], x_angle=theta[2])
- self.OBB[obb_to_move].T = np.vstack(
- [np.column_stack([self.OBB[obb_to_move].O.T, -self.OBB[obb_to_move].O.T @ self.OBB[obb_to_move].P]),
- [0, 0, 0, 1]])
- return self.OBB[obb_to_move], ori[0]
-
-
-if __name__ == '__main__':
- newenv = env()
+# this is the three dimensional configuration space for rrt
+# !/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: yue qi
+"""
+import numpy as np
+
+
+# from utils3D import OBB2AABB
+
+def R_matrix(z_angle, y_angle, x_angle):
+ # x angle: row; y angle: pitch; z angle: yaw
+ # generate rotation matrix in SO3
+ # RzRyRx = R, ZYX intrinsic rotation
+ # also (r1,r2,r3) in R3*3 in {W} frame
+ # used in obb.O
+ # [[R p]
+ # [0T 1]] gives transformation from body to world
+ return np.array(
+ [[np.cos(z_angle), -np.sin(z_angle), 0.0], [np.sin(z_angle), np.cos(z_angle), 0.0], [0.0, 0.0, 1.0]]) @ \
+ np.array(
+ [[np.cos(y_angle), 0.0, np.sin(y_angle)], [0.0, 1.0, 0.0], [-np.sin(y_angle), 0.0, np.cos(y_angle)]]) @ \
+ np.array(
+ [[1.0, 0.0, 0.0], [0.0, np.cos(x_angle), -np.sin(x_angle)], [0.0, np.sin(x_angle), np.cos(x_angle)]])
+
+
+def getblocks():
+ # AABBs
+ block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00],
+ [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00],
+ # [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00],
+ # [1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00],
+ # [6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00],
+ [1.21e+01, 0.00e+00, 0.00e+00, 1.29e+01, 5.00e+00, 3.90e+00],
+ [1.81e+01, 0.00e+00, 0.00e+00, 1.89e+01, 5.00e+00, 3.90e+00]]
+ Obstacles = []
+ for i in block:
+ i = np.array(i)
+ Obstacles.append([j for j in i])
+ return np.array(Obstacles)
+
+
+def getAABB(blocks):
+ # used for Pyrr package for detecting collision
+ AABB = []
+ for i in blocks:
+ AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger
+ return AABB
+
+
+class aabb(object):
+ # make AABB out of blocks,
+ # P: center point
+ # E: extents
+ # O: Rotation matrix in SO(3), in {w}
+ def __init__(self, AABB):
+ self.P = [(AABB[3] + AABB[0]) / 2, (AABB[4] + AABB[1]) / 2, (AABB[5] + AABB[2]) / 2] # center point
+ self.E = [(AABB[3] - AABB[0]) / 2, (AABB[4] - AABB[1]) / 2, (AABB[5] - AABB[2]) / 2] # extents
+ self.O = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
+
+
+class obb(object):
+ # P: center point
+ # E: extents
+ # O: Rotation matrix in SO(3), in {w}
+ def __init__(self, P, E, O):
+ self.P = P
+ self.E = E
+ self.O = O
+ self.T = np.vstack([np.column_stack([self.O.T, -self.O.T @ self.P]), [0, 0, 0, 1]])
+
+
+def getAABB2(blocks):
+ # used in lineAABB
+ AABB = []
+ for i in blocks:
+ AABB.append(aabb(i))
+ return AABB
+
+
+def getballs():
+ spheres = [[16, 2.5, 4, 2], [10, 2.5, 1, 1]]
+ Obstacles = []
+ for i in spheres:
+ Obstacles.append([j for j in i])
+ return np.array(Obstacles)
+
+
+def add_block(block=[1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]):
+ return block
+
+
+class env():
+ def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1):
+ self.resolution = resolution
+ self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax])
+ self.blocks = getblocks()
+ self.AABB = getAABB2(self.blocks)
+ self.AABB_pyrr = getAABB(self.blocks)
+ self.balls = getballs()
+ self.OBB = np.array([obb([2.6, 2.5, 1], [0.2, 2, 1], R_matrix(0, 0, 45))])
+ # self.OBB = np.squeeze(np.vstack([self.OBB,OBB2AABB(self.OBB[0])]))
+ # print(self.OBB)
+ # self.OBB = []
+ self.start = np.array([0.5, 2.5, 5.5])
+ self.goal = np.array([19.0, 2.5, 5.5])
+ self.t = 0 # time
+
+ def New_block(self):
+ newblock = add_block()
+ self.blocks = np.vstack([self.blocks, newblock])
+ self.AABB = getAABB2(self.blocks)
+ self.AABB_pyrr = getAABB(self.blocks)
+
+ def move_start(self, x):
+ self.start = x
+
+ def move_block(self, a=[0, 0, 0], s=0, v=[0.1, 0, 0], theta=[0, 0, 0], block_to_move=0, obb_to_move=0,
+ mode='uniform'):
+ # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time,
+ # R is an orthorgonal transform in R3*3, is the rotation matrix
+ # (x',t') = (x + tv, t) is uniform transformation
+ if mode == 'uniform':
+ ori = np.array(self.blocks[block_to_move])
+ self.blocks[block_to_move] = \
+ np.array([ori[0] + self.t * v[0],
+ ori[1] + self.t * v[1],
+ ori[2] + self.t * v[2],
+ ori[3] + self.t * v[0],
+ ori[4] + self.t * v[1],
+ ori[5] + self.t * v[2]])
+
+ self.AABB[block_to_move].P = \
+ [self.AABB[block_to_move].P[0] + self.t * v[0],
+ self.AABB[block_to_move].P[1] + self.t * v[1],
+ self.AABB[block_to_move].P[2] + self.t * v[2]]
+ # return a range of block that the block might moved
+ a = self.blocks[block_to_move]
+ # return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \
+ # a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]). \
+ # np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \
+ # ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
+ return a, ori
+ # (x',t') = (x + a, t + s) is a translation
+ if mode == 'translation':
+ ori = np.array(self.blocks[block_to_move])
+ self.blocks[block_to_move] = \
+ np.array([ori[0] + a[0],
+ ori[1] + a[1],
+ ori[2] + a[2],
+ ori[3] + a[0],
+ ori[4] + a[1],
+ ori[5] + a[2]])
+
+ self.AABB[block_to_move].P = \
+ [self.AABB[block_to_move].P[0] + a[0],
+ self.AABB[block_to_move].P[1] + a[1],
+ self.AABB[block_to_move].P[2] + a[2]]
+ self.t += s
+ # return a range of block that the block might moved
+ a = self.blocks[block_to_move]
+ return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution,
+ a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \
+ np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution,
+ ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
+ # return a,ori
+ # (x',t') = (Rx, t)
+ if mode == 'rotation': # this makes an OBB rotate
+ ori = [self.OBB[obb_to_move]]
+ self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0], y_angle=theta[1], x_angle=theta[2])
+ self.OBB[obb_to_move].T = np.vstack(
+ [np.column_stack([self.OBB[obb_to_move].O.T, -self.OBB[obb_to_move].O.T @ self.OBB[obb_to_move].P]),
+ [0, 0, 0, 1]])
+ return self.OBB[obb_to_move], ori[0]
+
+
+if __name__ == '__main__':
+ newenv = env()
diff --git a/Search-based Planning/Search_3D/plot_util3D.py b/Search_based_Planning/Search_3D/plot_util3D.py
similarity index 97%
rename from Search-based Planning/Search_3D/plot_util3D.py
rename to Search_based_Planning/Search_3D/plot_util3D.py
index 6bf565b..ad4c5f0 100644
--- a/Search-based Planning/Search_3D/plot_util3D.py
+++ b/Search_based_Planning/Search_3D/plot_util3D.py
@@ -1,174 +1,174 @@
-# plotting
-import matplotlib.pyplot as plt
-from mpl_toolkits.mplot3d import Axes3D
-from mpl_toolkits.mplot3d.art3d import Poly3DCollection
-import mpl_toolkits.mplot3d as plt3d
-from mpl_toolkits.mplot3d import proj3d
-import numpy as np
-
-def CreateSphere(center,r):
- u = np.linspace(0,2* np.pi,30)
- v = np.linspace(0,np.pi,30)
- x = np.outer(np.cos(u),np.sin(v))
- y = np.outer(np.sin(u),np.sin(v))
- z = np.outer(np.ones(np.size(u)),np.cos(v))
- x, y, z = r*x + center[0], r*y + center[1], r*z + center[2]
- return (x,y,z)
-
-def draw_Spheres(ax,balls):
- for i in balls:
- (xs,ys,zs) = CreateSphere(i[0:3],i[-1])
- ax.plot_wireframe(xs, ys, zs, alpha=0.15,color="b")
-
-def draw_block_list(ax, blocks ,color=None,alpha=0.15):
- '''
- drawing the blocks on the graph
- '''
- v = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]],
- dtype='float')
- f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]])
- n = blocks.shape[0]
- d = blocks[:, 3:6] - blocks[:, :3]
- vl = np.zeros((8 * n, 3))
- fl = np.zeros((6 * n, 4), dtype='int64')
- for k in range(n):
- vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3]
- fl[k * 6:(k + 1) * 6, :] = f + k * 8
- if type(ax) is Poly3DCollection:
- ax.set_verts(vl[fl])
- else:
- pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k')
- pc.set_facecolor(color)
- h = ax.add_collection3d(pc)
- return h
-
-def obb_verts(obb):
- # 0.017004013061523438 for 1000 iters
- ori_body = np.array([[1,1,1],[-1,1,1],[-1,-1,1],[1,-1,1],\
- [1,1,-1],[-1,1,-1],[-1,-1,-1],[1,-1,-1]])
- # P + (ori * E)
- ori_body = np.multiply(ori_body,obb.E)
- # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3)
- verts = (obb.O@ori_body.T).T + obb.P
- return verts
-
-
-def draw_obb(ax, OBB, color=None,alpha=0.15):
- f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]])
- n = OBB.shape[0]
- vl = np.zeros((8 * n, 3))
- fl = np.zeros((6 * n, 4), dtype='int64')
- for k in range(n):
- vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k])
- fl[k * 6:(k + 1) * 6, :] = f + k * 8
- if type(ax) is Poly3DCollection:
- ax.set_verts(vl[fl])
- else:
- pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k')
- pc.set_facecolor(color)
- h = ax.add_collection3d(pc)
- return h
-
-
-def draw_line(ax,SET,visibility=1,color=None):
- if SET != []:
- for i in SET:
- xs = i[0][0], i[1][0]
- ys = i[0][1], i[1][1]
- zs = i[0][2], i[1][2]
- line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color)
- ax.add_line(line)
-
-def visualization(initparams):
- if initparams.ind % 20 == 0 or initparams.done:
- V = np.array(list(initparams.V))
- # E = initparams.E
- Path = np.array(initparams.Path)
- start = initparams.env.start
- goal = initparams.env.goal
- # edges = E.get_edge()
- # generate axis objects
- ax = plt.subplot(111, projection='3d')
- #ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
- #ax.view_init(elev=0., azim=90.)
- ax.view_init(elev=8., azim=120.)
- #ax.view_init(elev=-8., azim=180)
- ax.clear()
- # drawing objects
- draw_Spheres(ax, initparams.env.balls)
- draw_block_list(ax, initparams.env.blocks)
- if initparams.env.OBB is not None:
- draw_obb(ax,initparams.env.OBB)
- draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0)
- # draw_line(ax,edges,visibility=0.25)
- draw_line(ax,Path,color='r')
- if len(V) > 0:
- ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',)
- ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
- ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
- # adjust the aspect ratio
- xmin, xmax = initparams.env.boundary[0], initparams.env.boundary[3]
- ymin, ymax = initparams.env.boundary[1], initparams.env.boundary[4]
- zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5]
- dx, dy, dz = xmax-xmin, ymax-ymin, zmax-zmin
- ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy)
- plt.xlabel('x')
- plt.ylabel('y')
- plt.pause(0.0001)
-
-def make_get_proj(self, rx, ry, rz):
- '''
- Return a variation on :func:`~mpl_toolkit.mplot2d.axes3d.Axes3D.getproj` that
- makes the box aspect ratio equal to *rx:ry:rz*, using an axes object *self*.
- '''
-
- rm = max(rx, ry, rz)
- kx = rm / rx; ky = rm / ry; kz = rm / rz
-
- # Copied directly from mpl_toolkit/mplot3d/axes3d.py. New or modified lines are
- # marked by ##
- def get_proj():
- relev, razim = np.pi * self.elev/180, np.pi * self.azim/180
-
- xmin, xmax = self.get_xlim3d()
- ymin, ymax = self.get_ylim3d()
- zmin, zmax = self.get_zlim3d()
-
- # transform to uniform world coordinates 0-1.0,0-1.0,0-1.0
- worldM = proj3d.world_transformation(xmin, xmax,
- ymin, ymax,
- zmin, zmax)
- ratio = 0.5
- # adjust the aspect ratio ##
- aspectM = proj3d.world_transformation(-kx + 1, kx, ##
- -ky + 1, ky, ##
- -kz + 1, kz) ##
-
- # look into the middle of the new coordinates
- R = np.array([0.5, 0.5, 0.5])
-
- xp = R[0] + np.cos(razim) * np.cos(relev) * self.dist *ratio
- yp = R[1] + np.sin(razim) * np.cos(relev) * self.dist *ratio
- zp = R[2] + np.sin(relev) * self.dist *ratio
- E = np.array((xp, yp, zp))
-
- self.eye = E
- self.vvec = R - E
- self.vvec = self.vvec / np.linalg.norm(self.vvec)
-
- if abs(relev) > np.pi/2:
- # upside down
- V = np.array((0, 0, -1))
- else:
- V = np.array((0, 0, 1))
- zfront, zback = -self.dist *ratio, self.dist *ratio
-
- viewM = proj3d.view_transformation(E, R, V)
- perspM = proj3d.persp_transformation(zfront, zback)
- M0 = np.dot(viewM, np.dot(aspectM, worldM)) ##
- M = np.dot(perspM, M0)
- return M
- return get_proj
-
-if __name__ == '__main__':
+# plotting
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from mpl_toolkits.mplot3d.art3d import Poly3DCollection
+import mpl_toolkits.mplot3d as plt3d
+from mpl_toolkits.mplot3d import proj3d
+import numpy as np
+
+def CreateSphere(center,r):
+ u = np.linspace(0,2* np.pi,30)
+ v = np.linspace(0,np.pi,30)
+ x = np.outer(np.cos(u),np.sin(v))
+ y = np.outer(np.sin(u),np.sin(v))
+ z = np.outer(np.ones(np.size(u)),np.cos(v))
+ x, y, z = r*x + center[0], r*y + center[1], r*z + center[2]
+ return (x,y,z)
+
+def draw_Spheres(ax,balls):
+ for i in balls:
+ (xs,ys,zs) = CreateSphere(i[0:3],i[-1])
+ ax.plot_wireframe(xs, ys, zs, alpha=0.15,color="b")
+
+def draw_block_list(ax, blocks ,color=None,alpha=0.15):
+ '''
+ drawing the blocks on the graph
+ '''
+ v = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]],
+ dtype='float')
+ f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]])
+ n = blocks.shape[0]
+ d = blocks[:, 3:6] - blocks[:, :3]
+ vl = np.zeros((8 * n, 3))
+ fl = np.zeros((6 * n, 4), dtype='int64')
+ for k in range(n):
+ vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3]
+ fl[k * 6:(k + 1) * 6, :] = f + k * 8
+ if type(ax) is Poly3DCollection:
+ ax.set_verts(vl[fl])
+ else:
+ pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k')
+ pc.set_facecolor(color)
+ h = ax.add_collection3d(pc)
+ return h
+
+def obb_verts(obb):
+ # 0.017004013061523438 for 1000 iters
+ ori_body = np.array([[1,1,1],[-1,1,1],[-1,-1,1],[1,-1,1],\
+ [1,1,-1],[-1,1,-1],[-1,-1,-1],[1,-1,-1]])
+ # P + (ori * E)
+ ori_body = np.multiply(ori_body,obb.E)
+ # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3)
+ verts = (obb.O@ori_body.T).T + obb.P
+ return verts
+
+
+def draw_obb(ax, OBB, color=None,alpha=0.15):
+ f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]])
+ n = OBB.shape[0]
+ vl = np.zeros((8 * n, 3))
+ fl = np.zeros((6 * n, 4), dtype='int64')
+ for k in range(n):
+ vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k])
+ fl[k * 6:(k + 1) * 6, :] = f + k * 8
+ if type(ax) is Poly3DCollection:
+ ax.set_verts(vl[fl])
+ else:
+ pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k')
+ pc.set_facecolor(color)
+ h = ax.add_collection3d(pc)
+ return h
+
+
+def draw_line(ax,SET,visibility=1,color=None):
+ if SET != []:
+ for i in SET:
+ xs = i[0][0], i[1][0]
+ ys = i[0][1], i[1][1]
+ zs = i[0][2], i[1][2]
+ line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color)
+ ax.add_line(line)
+
+def visualization(initparams):
+ if initparams.ind % 20 == 0 or initparams.done:
+ V = np.array(list(initparams.V))
+ # E = initparams.E
+ Path = np.array(initparams.Path)
+ start = initparams.env.start
+ goal = initparams.env.goal
+ # edges = E.get_edge()
+ # generate axis objects
+ ax = plt.subplot(111, projection='3d')
+ #ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
+ #ax.view_init(elev=0., azim=90.)
+ ax.view_init(elev=8., azim=120.)
+ #ax.view_init(elev=-8., azim=180)
+ ax.clear()
+ # drawing objects
+ draw_Spheres(ax, initparams.env.balls)
+ draw_block_list(ax, initparams.env.blocks)
+ if initparams.env.OBB is not None:
+ draw_obb(ax,initparams.env.OBB)
+ draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0)
+ # draw_line(ax,edges,visibility=0.25)
+ draw_line(ax,Path,color='r')
+ if len(V) > 0:
+ ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',)
+ ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
+ ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
+ # adjust the aspect ratio
+ xmin, xmax = initparams.env.boundary[0], initparams.env.boundary[3]
+ ymin, ymax = initparams.env.boundary[1], initparams.env.boundary[4]
+ zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5]
+ dx, dy, dz = xmax-xmin, ymax-ymin, zmax-zmin
+ ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy)
+ plt.xlabel('x')
+ plt.ylabel('y')
+ plt.pause(0.0001)
+
+def make_get_proj(self, rx, ry, rz):
+ '''
+ Return a variation on :func:`~mpl_toolkit.mplot2d.axes3d.Axes3D.getproj` that
+ makes the box aspect ratio equal to *rx:ry:rz*, using an axes object *self*.
+ '''
+
+ rm = max(rx, ry, rz)
+ kx = rm / rx; ky = rm / ry; kz = rm / rz
+
+ # Copied directly from mpl_toolkit/mplot3d/axes3d.py. New or modified lines are
+ # marked by ##
+ def get_proj():
+ relev, razim = np.pi * self.elev/180, np.pi * self.azim/180
+
+ xmin, xmax = self.get_xlim3d()
+ ymin, ymax = self.get_ylim3d()
+ zmin, zmax = self.get_zlim3d()
+
+ # transform to uniform world coordinates 0-1.0,0-1.0,0-1.0
+ worldM = proj3d.world_transformation(xmin, xmax,
+ ymin, ymax,
+ zmin, zmax)
+ ratio = 0.5
+ # adjust the aspect ratio ##
+ aspectM = proj3d.world_transformation(-kx + 1, kx, ##
+ -ky + 1, ky, ##
+ -kz + 1, kz) ##
+
+ # look into the middle of the new coordinates
+ R = np.array([0.5, 0.5, 0.5])
+
+ xp = R[0] + np.cos(razim) * np.cos(relev) * self.dist *ratio
+ yp = R[1] + np.sin(razim) * np.cos(relev) * self.dist *ratio
+ zp = R[2] + np.sin(relev) * self.dist *ratio
+ E = np.array((xp, yp, zp))
+
+ self.eye = E
+ self.vvec = R - E
+ self.vvec = self.vvec / np.linalg.norm(self.vvec)
+
+ if abs(relev) > np.pi/2:
+ # upside down
+ V = np.array((0, 0, -1))
+ else:
+ V = np.array((0, 0, 1))
+ zfront, zback = -self.dist *ratio, self.dist *ratio
+
+ viewM = proj3d.view_transformation(E, R, V)
+ perspM = proj3d.persp_transformation(zfront, zback)
+ M0 = np.dot(viewM, np.dot(aspectM, worldM)) ##
+ M = np.dot(perspM, M0)
+ return M
+ return get_proj
+
+if __name__ == '__main__':
pass
\ No newline at end of file
diff --git a/Search-based Planning/Search_3D/queue.py b/Search_based_Planning/Search_3D/queue.py
similarity index 100%
rename from Search-based Planning/Search_3D/queue.py
rename to Search_based_Planning/Search_3D/queue.py
diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search_based_Planning/Search_3D/utils3D.py
similarity index 99%
rename from Search-based Planning/Search_3D/utils3D.py
rename to Search_based_Planning/Search_3D/utils3D.py
index b421f35..116282f 100644
--- a/Search-based Planning/Search_3D/utils3D.py
+++ b/Search_based_Planning/Search_3D/utils3D.py
@@ -336,7 +336,7 @@ def cost(initparams, i, j, dist=None, settings='Euclidean'):
def initcost(initparams):
- # initialize cost dictionary, could be modifed lateron
+ # initialize Cost dictionary, could be modifed lateron
c = defaultdict(lambda: defaultdict(dict)) # two key dicionary
for xi in initparams.X:
cdren = children(initparams, xi)
diff --git a/Search-based Planning/gif/ADstar_sig.gif b/Search_based_Planning/gif/ADstar_sig.gif
similarity index 100%
rename from Search-based Planning/gif/ADstar_sig.gif
rename to Search_based_Planning/gif/ADstar_sig.gif
diff --git a/Search-based Planning/gif/ADstar_small.gif b/Search_based_Planning/gif/ADstar_small.gif
similarity index 100%
rename from Search-based Planning/gif/ADstar_small.gif
rename to Search_based_Planning/gif/ADstar_small.gif
diff --git a/Search-based Planning/gif/ARA_star.gif b/Search_based_Planning/gif/ARA_star.gif
similarity index 100%
rename from Search-based Planning/gif/ARA_star.gif
rename to Search_based_Planning/gif/ARA_star.gif
diff --git a/Search-based Planning/gif/Astar.gif b/Search_based_Planning/gif/Astar.gif
similarity index 100%
rename from Search-based Planning/gif/Astar.gif
rename to Search_based_Planning/gif/Astar.gif
diff --git a/Search-based Planning/gif/BF.gif b/Search_based_Planning/gif/BF.gif
similarity index 100%
rename from Search-based Planning/gif/BF.gif
rename to Search_based_Planning/gif/BF.gif
diff --git a/Search-based Planning/gif/BFS.gif b/Search_based_Planning/gif/BFS.gif
similarity index 100%
rename from Search-based Planning/gif/BFS.gif
rename to Search_based_Planning/gif/BFS.gif
diff --git a/Search-based Planning/gif/Bi-Astar.gif b/Search_based_Planning/gif/Bi-Astar.gif
similarity index 100%
rename from Search-based Planning/gif/Bi-Astar.gif
rename to Search_based_Planning/gif/Bi-Astar.gif
diff --git a/Search-based Planning/gif/DFS.gif b/Search_based_Planning/gif/DFS.gif
similarity index 100%
rename from Search-based Planning/gif/DFS.gif
rename to Search_based_Planning/gif/DFS.gif
diff --git a/Search-based Planning/gif/D_star.gif b/Search_based_Planning/gif/D_star.gif
similarity index 100%
rename from Search-based Planning/gif/D_star.gif
rename to Search_based_Planning/gif/D_star.gif
diff --git a/Search-based Planning/gif/D_star_Lite.gif b/Search_based_Planning/gif/D_star_Lite.gif
similarity index 100%
rename from Search-based Planning/gif/D_star_Lite.gif
rename to Search_based_Planning/gif/D_star_Lite.gif
diff --git a/Search-based Planning/gif/Dijkstra.gif b/Search_based_Planning/gif/Dijkstra.gif
similarity index 100%
rename from Search-based Planning/gif/Dijkstra.gif
rename to Search_based_Planning/gif/Dijkstra.gif
diff --git a/Search-based Planning/gif/LPA_star.gif b/Search_based_Planning/gif/LPA_star.gif
similarity index 100%
rename from Search-based Planning/gif/LPA_star.gif
rename to Search_based_Planning/gif/LPA_star.gif
diff --git a/Search-based Planning/gif/LPAstar.gif b/Search_based_Planning/gif/LPAstar.gif
similarity index 100%
rename from Search-based Planning/gif/LPAstar.gif
rename to Search_based_Planning/gif/LPAstar.gif
diff --git a/Search-based Planning/gif/LRTA_star.gif b/Search_based_Planning/gif/LRTA_star.gif
similarity index 100%
rename from Search-based Planning/gif/LRTA_star.gif
rename to Search_based_Planning/gif/LRTA_star.gif
diff --git a/Search-based Planning/gif/RTAA_star.gif b/Search_based_Planning/gif/RTAA_star.gif
similarity index 100%
rename from Search-based Planning/gif/RTAA_star.gif
rename to Search_based_Planning/gif/RTAA_star.gif
diff --git a/Search-based Planning/gif/RepeatedA_star.gif b/Search_based_Planning/gif/RepeatedA_star.gif
similarity index 100%
rename from Search-based Planning/gif/RepeatedA_star.gif
rename to Search_based_Planning/gif/RepeatedA_star.gif
diff --git a/Stochastic Shortest Path/.idea/.gitignore b/Stochastic Shortest Path/.idea/.gitignore
deleted file mode 100644
index 0e40fe8..0000000
--- a/Stochastic Shortest Path/.idea/.gitignore
+++ /dev/null
@@ -1,3 +0,0 @@
-
-# Default ignored files
-/workspace.xml
\ No newline at end of file
diff --git a/Stochastic Shortest Path/.idea/inspectionProfiles/profiles_settings.xml b/Stochastic Shortest Path/.idea/inspectionProfiles/profiles_settings.xml
deleted file mode 100644
index 105ce2d..0000000
--- a/Stochastic Shortest Path/.idea/inspectionProfiles/profiles_settings.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Stochastic Shortest Path/.idea/misc.xml b/Stochastic Shortest Path/.idea/misc.xml
deleted file mode 100644
index 0e7ac62..0000000
--- a/Stochastic Shortest Path/.idea/misc.xml
+++ /dev/null
@@ -1,4 +0,0 @@
-
-
-
-
\ No newline at end of file
diff --git a/Stochastic Shortest Path/.idea/modules.xml b/Stochastic Shortest Path/.idea/modules.xml
deleted file mode 100644
index f86c9be..0000000
--- a/Stochastic Shortest Path/.idea/modules.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Stochastic Shortest Path/.idea/vcs.xml b/Stochastic Shortest Path/.idea/vcs.xml
deleted file mode 100644
index 6c0b863..0000000
--- a/Stochastic Shortest Path/.idea/vcs.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Stochastic Shortest Path/Q-policy_iteration.py b/Stochastic Shortest Path/Q-policy_iteration.py
deleted file mode 100644
index f29cee0..0000000
--- a/Stochastic Shortest Path/Q-policy_iteration.py
+++ /dev/null
@@ -1,155 +0,0 @@
-import env
-import plotting
-import motion_model
-
-import numpy as np
-import copy
-import sys
-
-
-class Q_policy_iteration:
- def __init__(self, x_start, x_goal):
- self.xI, self.xG = x_start, x_goal
- self.e = 0.001 # threshold for convergence
- self.gamma = 0.9 # discount factor
-
- self.env = env.Env(self.xI, self.xG) # class Env
- self.motion = motion_model.Motion_model(self.xI, self.xG) # class Motion_model
- self.plotting = plotting.Plotting(self.xI, self.xG) # class Plotting
-
- 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-policy_iteration, gamma=" + str(self.gamma)
-
- [self.value, self.policy] = self.iteration()
- self.path = self.extract_path(self.xI, self.xG, self.policy)
- self.plotting.animation(self.path, self.name1)
-
- def policy_evaluation(self, policy, value):
- """
- evaluation process using current policy.
-
- :param policy: current policy
- :param value: value table
- :return: converged value table
- """
-
- delta = sys.maxsize
-
- while delta > self.e: # convergence condition
- x_value = 0
- for x in value:
- if x not in self.xG:
- for k in range(len(self.u_set)):
- [x_next, p_next] = self.motion.move_next(x, self.u_set[k])
- v_Q = self.cal_Q_value(x_next, p_next, policy, value)
- v_diff = abs(value[x][k] - v_Q)
- value[x][k] = v_Q
- if v_diff > 0:
- x_value = max(x_value, v_diff)
- delta = x_value
-
- return value
-
- def policy_improvement(self, policy, value):
- """
- policy improvement process.
-
- :param policy: policy table
- :param value: current value table
- :return: improved policy
- """
-
- for x in self.stateSpace:
- if x not in self.xG:
- policy[x] = int(np.argmax(value[x]))
-
- return policy
-
- def iteration(self):
- """
- Q-policy iteration
- :return: converged policy and its value table.
- """
-
- Q_table = {}
- policy = {}
- count = 0
-
- for x in self.stateSpace:
- Q_table[x] = [0, 0, 0, 0] # initialize Q_value table
- policy[x] = 0 # initialize policy table
-
- while True:
- count += 1
- policy_back = copy.deepcopy(policy)
- Q_table = self.policy_evaluation(policy, Q_table) # evaluation process
- policy = self.policy_improvement(policy, Q_table) # improvement process
- if policy_back == policy: break # convergence condition
-
- self.message(count)
-
- return Q_table, policy
-
- def cal_Q_value(self, x, p, policy, table):
- """
- cal Q_value.
-
- :param x: next state vector
- :param p: probability of each state
- :param table: value table
- :return: Q-value
- """
-
- value = 0
- reward = self.env.get_reward(x) # get reward of next state
- for i in range(len(x)):
- value += p[i] * (reward[i] + self.gamma * table[x[i]][policy[x[i]]])
-
- return value
-
- 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 not in 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, count):
- """
- print important message.
-
- :param count: iteration numbers
- :return: print
- """
-
- print("starting state: ", self.xI)
- print("goal states: ", self.xG)
- print("condition for convergence: ", self.e)
- print("discount factor: ", self.gamma)
- print("iteration times: ", count)
-
-
-if __name__ == '__main__':
- x_Start = (5, 5)
- x_Goal = [(49, 5), (49, 25)]
-
- QPI = Q_policy_iteration(x_Start, x_Goal)
diff --git a/Stochastic Shortest Path/Q-value_iteration.py b/Stochastic Shortest Path/Q-value_iteration.py
deleted file mode 100644
index a8bf45a..0000000
--- a/Stochastic Shortest Path/Q-value_iteration.py
+++ /dev/null
@@ -1,128 +0,0 @@
-import env
-import plotting
-import motion_model
-
-import numpy as np
-import sys
-
-
-class Q_value_iteration:
- def __init__(self, x_start, x_goal):
- self.xI, self.xG = x_start, x_goal
- self.e = 0.001 # threshold for convergence
- self.gamma = 0.9 # discount factor
-
- self.env = env.Env(self.xI, self.xG) # class Env
- self.motion = motion_model.Motion_model(self.xI, self.xG) # class Motion_model
- self.plotting = plotting.Plotting(self.xI, self.xG) # class Plotting
-
- 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-value_iteration, gamma=" + str(self.gamma)
- self.name2 = "converge process, e=" + str(self.e)
-
- [self.value, self.policy, self.diff] = self.iteration(self.xI, self.xG)
- self.path = self.extract_path(self.xI, self.xG, self.policy)
- self.plotting.animation(self.path, self.name1)
- self.plotting.plot_diff(self.diff, self.name2)
-
- def iteration(self, xI, xG):
- """
- Q_value_iteration
- :return: converged Q table and policy
- """
-
- Q_table = {}
- policy = {}
- diff = []
- delta = sys.maxsize
- count = 0
-
- for x in self.stateSpace:
- Q_table[x] = [0, 0, 0, 0] # initialize Q_table
-
- while delta > self.e: # convergence condition
- count += 1
- x_value = 0
- for x in self.stateSpace:
- if x not in x_Goal:
- for k in range(len(self.u_set)):
- [x_next, p_next] = self.motion.move_next(x, self.u_set[k])
- Q_value = self.cal_Q_value(x_next, p_next, Q_table)
- v_diff = abs(Q_table[x][k] - Q_value)
- Q_table[x][k] = Q_value
- if v_diff > 0:
- x_value = max(x_value, v_diff)
- diff.append(x_value)
- delta = x_value
-
- for x in self.stateSpace:
- if x not in xG:
- policy[x] = np.argmax(Q_table[x])
-
- self.message(count)
-
- return Q_table, policy, diff
-
- def cal_Q_value(self, x, p, table):
- """
- cal Q_value.
-
- :param x: next state vector
- :param p: probability of each state
- :param table: value table
- :return: Q-value
- """
-
- value = 0
- reward = self.env.get_reward(x) # get reward of next state
- for i in range(len(x)):
- value += p[i] * (reward[i] + self.gamma * max(table[x[i]]))
-
- return value
-
- 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 not in 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, count):
- """
- print important message.
-
- :param count: iteration numbers
- :return: print
- """
-
- print("starting state: ", self.xI)
- print("goal states: ", self.xG)
- print("condition for convergence: ", self.e)
- print("discount factor: ", self.gamma)
- print("iteration times: ", count)
-
-
-if __name__ == '__main__':
- x_Start = (5, 5)
- x_Goal = [(49, 5), (49, 25)]
-
- QVI = Q_value_iteration(x_Start, x_Goal)
diff --git a/Stochastic Shortest Path/__pycache__/env.cpython-37.pyc b/Stochastic Shortest Path/__pycache__/env.cpython-37.pyc
deleted file mode 100644
index b12fe16..0000000
Binary files a/Stochastic Shortest Path/__pycache__/env.cpython-37.pyc and /dev/null differ
diff --git a/Stochastic Shortest Path/__pycache__/motion_model.cpython-37.pyc b/Stochastic Shortest Path/__pycache__/motion_model.cpython-37.pyc
deleted file mode 100644
index 2798fa6..0000000
Binary files a/Stochastic Shortest Path/__pycache__/motion_model.cpython-37.pyc and /dev/null differ
diff --git a/Stochastic Shortest Path/__pycache__/plotting.cpython-37.pyc b/Stochastic Shortest Path/__pycache__/plotting.cpython-37.pyc
deleted file mode 100644
index 047d6bd..0000000
Binary files a/Stochastic Shortest Path/__pycache__/plotting.cpython-37.pyc and /dev/null differ
diff --git a/Stochastic Shortest Path/env.py b/Stochastic Shortest Path/env.py
deleted file mode 100644
index fb4fa89..0000000
--- a/Stochastic Shortest Path/env.py
+++ /dev/null
@@ -1,87 +0,0 @@
-class Env:
- def __init__(self, xI, xG):
- self.x_range = 51 # size of background
- self.y_range = 31
- 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))
-
- for i in range(10, 21):
- obs.append((i, 15))
- for i in range(15):
- obs.append((20, i))
-
- for i in range(15, 30):
- obs.append((30, i))
- for i in range(16):
- obs.append((40, i))
-
- return obs
-
- def lose_map(self):
- """
- Initialize losing states' positions
- :return: losing states
- """
-
- lose = []
- for i in range(25, 36):
- lose.append((i, 13))
-
- 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
- """
-
- reward = []
- for x in x_next:
- if x in self.xG:
- reward.append(10) # reward : 10, for goal states
- elif x in self.lose:
- reward.append(-10) # reward : -10, for lose states
- else:
- reward.append(0) # reward : 0, for other states
-
- return reward
diff --git a/Stochastic Shortest Path/gif/VI.gif b/Stochastic Shortest Path/gif/VI.gif
deleted file mode 100644
index b45ce7d..0000000
Binary files a/Stochastic Shortest Path/gif/VI.gif and /dev/null differ
diff --git a/Stochastic Shortest Path/gif/VI.jpeg b/Stochastic Shortest Path/gif/VI.jpeg
deleted file mode 100644
index 72ae985..0000000
Binary files a/Stochastic Shortest Path/gif/VI.jpeg and /dev/null differ
diff --git a/Stochastic Shortest Path/gif/VI_E.gif b/Stochastic Shortest Path/gif/VI_E.gif
deleted file mode 100644
index a55a876..0000000
Binary files a/Stochastic Shortest Path/gif/VI_E.gif and /dev/null differ
diff --git a/Stochastic Shortest Path/motion_model.py b/Stochastic Shortest Path/motion_model.py
deleted file mode 100644
index 0285570..0000000
--- a/Stochastic Shortest Path/motion_model.py
+++ /dev/null
@@ -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
diff --git a/Stochastic Shortest Path/plotting.py b/Stochastic Shortest Path/plotting.py
deleted file mode 100644
index 05fdc34..0000000
--- a/Stochastic Shortest Path/plotting.py
+++ /dev/null
@@ -1,113 +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")
- for x in self.xG:
- plt.plot(x[0], x[1], "gs")
-
- plt.plot(obs_x, obs_y, "sk")
- 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')
-
- 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)
- for x in self.xG:
- if x in path:
- path.remove(x)
-
- for x in path:
- 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])
- 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()
diff --git a/Stochastic Shortest Path/policy_iteration.py b/Stochastic Shortest Path/policy_iteration.py
deleted file mode 100644
index 3c482c3..0000000
--- a/Stochastic Shortest Path/policy_iteration.py
+++ /dev/null
@@ -1,158 +0,0 @@
-import env
-import plotting
-import motion_model
-
-import numpy as np
-import sys
-import copy
-
-
-class Policy_iteration:
- def __init__(self, x_start, x_goal):
- self.xI, self.xG = x_start, x_goal
- self.e = 0.001 # threshold for convergence
- self.gamma = 0.9 # discount factor
-
- 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 = "policy_iteration, gamma=" + str(self.gamma)
-
- [self.value, self.policy] = self.iteration()
- self.path = self.extract_path(self.xI, self.xG, self.policy)
- self.plotting.animation(self.path, self.name1)
-
- def policy_evaluation(self, policy, value):
- """
- Evaluate current policy.
-
- :param policy: current policy
- :param value: value table
- :return: new value table generated by current policy
- """
-
- delta = sys.maxsize
-
- while delta > self.e: # convergence condition
- x_value = 0
- for x in self.stateSpace:
- if x not in self.xG:
- [x_next, p_next] = self.motion.move_next(x, policy[x])
- v_Q = self.cal_Q_value(x_next, p_next, value)
- v_diff = abs(value[x] - v_Q)
- value[x] = v_Q
- if v_diff > 0:
- x_value = max(x_value, v_diff)
- delta = x_value
-
- return value
-
- def policy_improvement(self, policy, value):
- """
- Improve policy using current value table.
-
- :param policy: policy table
- :param value: current value table
- :return: improved policy table
- """
-
- for x in self.stateSpace:
- if x not in self.xG:
- value_list = []
- for u in self.u_set:
- [x_next, p_next] = self.motion.move_next(x, u)
- value_list.append(self.cal_Q_value(x_next, p_next, value))
- policy[x] = self.u_set[int(np.argmax(value_list))]
-
- return policy
-
- def iteration(self):
- """
- polity iteration: using evaluate and improvement process until convergence.
- :return: value table and converged policy.
- """
-
- value_table = {}
- policy = {}
- count = 0
-
- for x in self.stateSpace:
- value_table[x] = 0 # initialize value table
- policy[x] = self.u_set[0] # initialize policy table
-
- while True:
- count += 1
- policy_back = copy.deepcopy(policy)
- value_table = self.policy_evaluation(policy, value_table) # evaluation process
- policy = self.policy_improvement(policy, value_table) # policy improvement process
- if policy_back == policy: break # convergence condition
-
- self.message(count)
-
- return value_table, policy
-
- def cal_Q_value(self, x, p, table):
- """
- cal Q_value.
-
- :param x: next state vector
- :param p: probability of each state
- :param table: value table
- :return: Q-value
- """
-
- value = 0
- reward = self.env.get_reward(x) # get reward of next state
- for i in range(len(x)):
- value += p[i] * (reward[i] + self.gamma * table[x[i]]) # cal Q-value
-
- return value
-
- 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 not in xG:
- u = 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, count):
- """
- print important message.
-
- :param count: iteration numbers
- :return: print
- """
-
- print("starting state: ", self.xI)
- print("goal states: ", self.xG)
- print("condition for convergence: ", self.e)
- print("discount factor: ", self.gamma)
- print("iteration times: ", count)
-
-
-if __name__ == '__main__':
- x_Start = (5, 5)
- x_Goal = [(49, 5), (49, 25)]
-
- PI = Policy_iteration(x_Start, x_Goal)
diff --git a/Stochastic Shortest Path/value_iteration.py b/Stochastic Shortest Path/value_iteration.py
deleted file mode 100644
index 932e4a2..0000000
--- a/Stochastic Shortest Path/value_iteration.py
+++ /dev/null
@@ -1,126 +0,0 @@
-import env
-import plotting
-import motion_model
-
-import numpy as np
-import sys
-
-
-class Value_iteration:
- def __init__(self, x_start, x_goal):
- self.xI, self.xG = x_start, x_goal
- self.e = 0.001 # threshold for convergence
- self.gamma = 0.9 # discount factor
-
- self.env = env.Env(self.xI, self.xG) # class Env
- self.motion = motion_model.Motion_model(self.xI, self.xG) # class Motion_model
- self.plotting = plotting.Plotting(self.xI, self.xG) # class Plotting
-
- 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 = "value_iteration, gamma=" + str(self.gamma)
- self.name2 = "converge process, e=" + str(self.e)
-
- [self.value, self.policy, self.diff] = self.iteration(self.xI, self.xG)
- self.path = self.extract_path(self.xI, self.xG, self.policy)
- self.plotting.animation(self.path, self.name1)
- self.plotting.plot_diff(self.diff, self.name2)
-
- def iteration(self, xI, xG):
- """
- value_iteration.
-
- :return: converged value table, optimal policy and variation of difference,
- """
-
- value_table = {} # value table
- policy = {} # policy
- diff = [] # maximum difference between two successive iteration
- delta = sys.maxsize # initialize maximum difference
- count = 0 # iteration times
-
- for x in self.stateSpace: # initialize value table for feasible states
- value_table[x] = 0
-
- while delta > self.e: # converged condition
- count += 1
- x_value = 0
- for x in self.stateSpace:
- if x not in xG:
- value_list = []
- for u in self.u_set:
- [x_next, p_next] = self.motion.move_next(x, u) # recall motion model
- value_list.append(self.cal_Q_value(x_next, p_next, value_table)) # cal Q value
- policy[x] = self.u_set[int(np.argmax(value_list))] # update policy
- v_diff = abs(value_table[x] - max(value_list)) # maximum difference
- value_table[x] = max(value_list) # update value table
- x_value = max(x_value, v_diff)
- delta = x_value # update delta
- diff.append(delta)
-
- self.message(count) # print messages
-
- return value_table, policy, diff
-
- def cal_Q_value(self, x, p, table):
- """
- cal Q_value.
-
- :param x: next state vector
- :param p: probability of each state
- :param table: value table
- :return: Q-value
- """
-
- value = 0
- reward = self.env.get_reward(x) # get reward of next state
- for i in range(len(x)):
- value += p[i] * (reward[i] + self.gamma * table[x[i]]) # cal Q-value
-
- return value
-
- 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 not in xG:
- u = 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, count):
- """
- print important message.
-
- :param count: iteration numbers
- :return: print
- """
-
- print("starting state: ", self.xI)
- print("goal states: ", self.xG)
- print("condition for convergence: ", self.e)
- print("discount factor: ", self.gamma)
- print("iteration times: ", count)
-
-
-if __name__ == '__main__':
- x_Start = (5, 5) # starting state
- x_Goal = [(49, 5), (49, 25)] # goal states
-
- VI = Value_iteration(x_Start, x_Goal)
