Showing preview only (413K chars total). Download the full file or copy to clipboard to get everything.
Repository: zhm-real/PathPlanning
Branch: master
Commit: 89c200e0ab85
Files: 75
Total size: 389.8 KB
Directory structure:
gitextract_yrm3dru6/
├── .idea/
│ ├── .gitignore
│ ├── PathPlanning.iml
│ ├── inspectionProfiles/
│ │ └── profiles_settings.xml
│ ├── misc.xml
│ ├── modules.xml
│ └── vcs.xml
├── CurvesGenerator/
│ ├── bezier_path.py
│ ├── bspline_curve.py
│ ├── cubic_spline.py
│ ├── draw.py
│ ├── dubins_path.py
│ ├── quartic_polynomial.py
│ ├── quintic_polynomial.py
│ └── reeds_shepp.py
├── LICENSE
├── README.md
├── Sampling_based_Planning/
│ ├── rrt_2D/
│ │ ├── adaptively_informed_trees.py
│ │ ├── advanced_batch_informed_trees.py
│ │ ├── batch_informed_trees.py
│ │ ├── dubins_rrt_star.py
│ │ ├── dynamic_rrt.py
│ │ ├── env.py
│ │ ├── extended_rrt.py
│ │ ├── fast_marching_trees.py
│ │ ├── informed_rrt_star.py
│ │ ├── plotting.py
│ │ ├── queue.py
│ │ ├── rrt.py
│ │ ├── rrt_connect.py
│ │ ├── rrt_sharp.py
│ │ ├── rrt_star.py
│ │ ├── rrt_star_smart.py
│ │ └── utils.py
│ └── rrt_3D/
│ ├── ABIT_star3D.py
│ ├── BIT_star3D.py
│ ├── FMT_star3D.py
│ ├── dynamic_rrt3D.py
│ ├── env3D.py
│ ├── extend_rrt3D.py
│ ├── informed_rrt_star3D.py
│ ├── plot_util3D.py
│ ├── queue.py
│ ├── rrt3D.py
│ ├── rrt_connect3D.py
│ ├── rrt_star3D.py
│ ├── rrt_star_smart3D.py
│ └── utils3D.py
└── Search_based_Planning/
├── Search_2D/
│ ├── ARAstar.py
│ ├── Anytime_D_star.py
│ ├── Astar.py
│ ├── Best_First.py
│ ├── Bidirectional_a_star.py
│ ├── D_star.py
│ ├── D_star_Lite.py
│ ├── Dijkstra.py
│ ├── LPAstar.py
│ ├── LRTAstar.py
│ ├── RTAAStar.py
│ ├── bfs.py
│ ├── dfs.py
│ ├── env.py
│ ├── plotting.py
│ └── queue.py
└── Search_3D/
├── Anytime_Dstar3D.py
├── Astar3D.py
├── Dstar3D.py
├── DstarLite3D.py
├── LP_Astar3D.py
├── LRT_Astar3D.py
├── RTA_Astar3D.py
├── bidirectional_Astar3D.py
├── env3D.py
├── plot_util3D.py
├── queue.py
└── utils3D.py
================================================
FILE CONTENTS
================================================
================================================
FILE: .idea/.gitignore
================================================
# Default ignored files
/workspace.xml
================================================
FILE: .idea/PathPlanning.iml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$" />
<orderEntry type="jdk" jdkName="Python 3.7" jdkType="Python SDK" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
<component name="TestRunnerService">
<option name="projectConfiguration" value="pytest" />
<option name="PROJECT_TEST_RUNNER" value="pytest" />
</component>
</module>
================================================
FILE: .idea/inspectionProfiles/profiles_settings.xml
================================================
<component name="InspectionProjectProfileManager">
<settings>
<option name="USE_PROJECT_PROFILE" value="false" />
<version value="1.0" />
</settings>
</component>
================================================
FILE: .idea/misc.xml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
</project>
================================================
FILE: .idea/modules.xml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/PathPlanning.iml" filepath="$PROJECT_DIR$/.idea/PathPlanning.iml" />
</modules>
</component>
</project>
================================================
FILE: .idea/vcs.xml
================================================
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="$PROJECT_DIR$" vcs="Git" />
</component>
</project>
================================================
FILE: CurvesGenerator/bezier_path.py
================================================
"""
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()
================================================
FILE: CurvesGenerator/bspline_curve.py
================================================
"""
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()
================================================
FILE: CurvesGenerator/cubic_spline.py
================================================
#! /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 s
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 s, 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 s, 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("s[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()
================================================
FILE: CurvesGenerator/draw.py
================================================
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)
================================================
FILE: CurvesGenerator/dubins_path.py
================================================
"""
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 s 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: (s, 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()
================================================
FILE: CurvesGenerator/quartic_polynomial.py
================================================
"""
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
================================================
FILE: CurvesGenerator/quintic_polynomial.py
================================================
"""
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()
================================================
FILE: CurvesGenerator/reeds_shepp.py
================================================
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 s 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 (s, 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: (s, 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()
================================================
FILE: LICENSE
================================================
MIT License
Copyright (c) 2020 Huiming Zhou
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
================================================
FILE: README.md
================================================
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. The related papers are listed in [Papers](https://github.com/zhm-real/PathPlanning#papers).
Directory Structure
------
.
└── Search-based Planning
├── 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*
└── Sampling-based Planning
├── RRT
├── RRT-Connect
├── Extended-RRT
├── Dynamic-RRT
├── RRT*
├── Informed RRT*
├── RRT* Smart
├── Anytime RRT*
├── Closed-Loop RRT*
├── Spline-RRT*
├── Fast Marching Trees (FMT*)
└── Batch Informed Trees (BIT*)
└── Papers
## Animations - Search-Based
### Best-First & Dijkstra
<div align=right>
<table>
<tr>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/BF.gif" alt="dfs" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/Dijkstra.gif" alt="dijkstra" width="400"/></a></td>
</tr>
</table>
</div>
### A* and A* Variants
<div align=right>
<table>
<tr>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/Astar.gif" alt="astar" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/Bi-Astar.gif" alt="biastar" width="400"/></a></td>
</tr>
</table>
<table>
<tr>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/RepeatedA_star.gif" alt="repeatedastar" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/ARA_star.gif" alt="arastar" width="400"/></a></td>
</tr>
</table>
<table>
<tr>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/LRTA_star.gif" alt="lrtastar" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/RTAA_star.gif" alt="rtaastar" width="400"/></a></td>
</tr>
</table>
<table>
<tr>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/D_star.gif" alt="lpastar" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/LPAstar.gif" alt="dstarlite" width="400"/></a></td>
</tr>
</table>
<table>
<tr>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/ADstar_small.gif" alt="lpastar" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/ADstar_sig.gif" alt="dstarlite" width="400"/></a></td>
</tr>
</table>
</div>
## Animation - Sampling-Based
### RRT & Variants
<div align=right>
<table>
<tr>
<td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/RRT_2D.gif" alt="value iteration" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/Goal_biasd_RRT_2D.gif" alt="value iteration" width="400"/></a></td>
</tr>
</table>
<table>
<tr>
<td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/RRT_CONNECT_2D.gif" alt="value iteration" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/Extended_RRT_2D.gif" alt="value iteration" width="400"/></a></td>
</tr>
</table>
<table>
<tr>
<td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/Dynamic_RRT_2D.gif" alt="value iteration" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/RRT_STAR2_2D.gif" alt="value iteration" width="400"/></a></td>
</tr>
</table>
<table>
<tr>
<td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/RRT_STAR_SMART_2D.gif" alt="value iteration" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/FMT.gif" alt="value iteration" width="400"/></a></td>
</tr>
</table>
<table>
<tr>
<td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D3.gif" alt="value iteration" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/PathPlanning/blob/master/Sampling_based_Planning/gif/BIT2.gif" alt="value iteration" width="400"/></a></td>
</tr>
</table>
</div>
## Papers
### Search-base Planning
* [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
* [Focussed D*: ](http://robotics.caltech.edu/~jwb/courses/ME132/handouts/Dstar_ijcai95.pdf) The Focussed D* Algorithm for Real-Time Replanning
* [Potential Field, ](https://journals.sagepub.com/doi/abs/10.1177/027836498600500106) [[PPT]: ](https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf) Real-Time Obstacle Avoidance for Manipulators and Mobile Robots
* [Hybrid A*: ](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf) Practical Search Techniques in Path Planning for Autonomous Driving
### Sampling-based Planning
* [RRT: ](http://msl.cs.uiuc.edu/~lavalle/papers/Lav98c.pdf) Rapidly-Exploring Random Trees: A New Tool for Path Planning
* [RRT-Connect: ](http://www-cgi.cs.cmu.edu/afs/cs/academic/class/15494-s12/readings/kuffner_icra2000.pdf) RRT-Connect: An Efficient Approach to Single-Query Path Planning
* [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
* [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)
================================================
FILE: Sampling_based_Planning/rrt_2D/adaptively_informed_trees.py
================================================
================================================
FILE: Sampling_based_Planning/rrt_2D/advanced_batch_informed_trees.py
================================================
================================================
FILE: Sampling_based_Planning/rrt_2D/batch_informed_trees.py
================================================
"""
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()
================================================
FILE: Sampling_based_Planning/rrt_2D/dubins_rrt_star.py
================================================
"""
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()
================================================
FILE: Sampling_based_Planning/rrt_2D/dynamic_rrt.py
================================================
"""
DYNAMIC_RRT_2D
@author: huiming zhou
"""
import os
import sys
import math
import copy
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.flag = "VALID"
class Edge:
def __init__(self, n_p, n_c):
self.parent = n_p
self.child = n_c
self.flag = "VALID"
class DynamicRrt:
def __init__(self, s_start, s_goal, step_len, goal_sample_rate, waypoint_sample_rate, iter_max):
self.s_start = Node(s_start)
self.s_goal = Node(s_goal)
self.step_len = step_len
self.goal_sample_rate = goal_sample_rate
self.waypoint_sample_rate = waypoint_sample_rate
self.iter_max = iter_max
self.vertex = [self.s_start]
self.vertex_old = []
self.vertex_new = []
self.edges = []
self.env = env.Env()
self.plotting = plotting.Plotting(s_start, s_goal)
self.utils = utils.Utils()
self.fig, self.ax = plt.subplots()
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.obs_add = [0, 0, 0]
self.path = []
self.waypoint = []
def planning(self):
for i in range(self.iter_max):
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 node_new and not self.utils.is_collision(node_near, node_new):
self.vertex.append(node_new)
self.edges.append(Edge(node_near, node_new))
dist, _ = self.get_distance_and_angle(node_new, self.s_goal)
if dist <= self.step_len:
self.new_state(node_new, self.s_goal)
path = self.extract_path(node_new)
self.plot_grid("Dynamic_RRT")
self.plot_visited()
self.plot_path(path)
self.path = path
self.waypoint = self.extract_waypoint(node_new)
self.fig.canvas.mpl_connect('button_press_event', self.on_press)
plt.show()
return
return None
def on_press(self, event):
x, y = event.xdata, event.ydata
if x < 0 or x > 50 or y < 0 or y > 30:
print("Please choose right area!")
else:
x, y = int(x), int(y)
print("Add circle obstacle at: s =", x, ",", "y =", y)
self.obs_add = [x, y, 2]
self.obs_circle.append([x, y, 2])
self.utils.update_obs(self.obs_circle, self.obs_boundary, self.obs_rectangle)
self.InvalidateNodes()
if self.is_path_invalid():
print("Path is Replanning ...")
path, waypoint = self.replanning()
print("len_vertex: ", len(self.vertex))
print("len_vertex_old: ", len(self.vertex_old))
print("len_vertex_new: ", len(self.vertex_new))
plt.cla()
self.plot_grid("Dynamic_RRT")
self.plot_vertex_old()
self.plot_path(self.path, color='blue')
self.plot_vertex_new()
self.vertex_new = []
self.plot_path(path)
self.path = path
self.waypoint = waypoint
else:
print("Trimming Invalid Nodes ...")
self.TrimRRT()
plt.cla()
self.plot_grid("Dynamic_RRT")
self.plot_visited(animation=False)
self.plot_path(self.path)
self.fig.canvas.draw_idle()
def InvalidateNodes(self):
for edge in self.edges:
if self.is_collision_obs_add(edge.parent, edge.child):
edge.child.flag = "INVALID"
def is_path_invalid(self):
for node in self.waypoint:
if node.flag == "INVALID":
return True
def is_collision_obs_add(self, start, end):
delta = self.utils.delta
obs_add = self.obs_add
if math.hypot(start.x - obs_add[0], start.y - obs_add[1]) <= obs_add[2] + delta:
return True
if math.hypot(end.x - obs_add[0], end.y - obs_add[1]) <= obs_add[2] + delta:
return True
o, d = self.utils.get_ray(start, end)
if self.utils.is_intersect_circle(o, d, [obs_add[0], obs_add[1]], obs_add[2]):
return True
return False
def replanning(self):
self.TrimRRT()
for i in range(self.iter_max):
node_rand = self.generate_random_node_replanning(self.goal_sample_rate, self.waypoint_sample_rate)
node_near = self.nearest_neighbor(self.vertex, node_rand)
node_new = self.new_state(node_near, node_rand)
if node_new and not self.utils.is_collision(node_near, node_new):
self.vertex.append(node_new)
self.vertex_new.append(node_new)
self.edges.append(Edge(node_near, node_new))
dist, _ = self.get_distance_and_angle(node_new, self.s_goal)
if dist <= self.step_len:
self.new_state(node_new, self.s_goal)
path = self.extract_path(node_new)
waypoint = self.extract_waypoint(node_new)
print("path: ", len(path))
print("waypoint: ", len(waypoint))
return path, waypoint
return None
def TrimRRT(self):
for i in range(1, len(self.vertex)):
node = self.vertex[i]
node_p = node.parent
if node_p.flag == "INVALID":
node.flag = "INVALID"
self.vertex = [node for node in self.vertex if node.flag == "VALID"]
self.vertex_old = copy.deepcopy(self.vertex)
self.edges = [Edge(node.parent, node) for node in self.vertex[1:len(self.vertex)]]
def generate_random_node(self, goal_sample_rate):
delta = self.utils.delta
if np.random.random() > goal_sample_rate:
return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
return self.s_goal
def generate_random_node_replanning(self, goal_sample_rate, waypoint_sample_rate):
delta = self.utils.delta
p = np.random.random()
if p < goal_sample_rate:
return self.s_goal
elif goal_sample_rate < p < goal_sample_rate + waypoint_sample_rate:
return self.waypoint[np.random.randint(0, len(self.waypoint) - 1)]
else:
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)))
@staticmethod
def nearest_neighbor(node_list, n):
return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
for nd in node_list]))]
def new_state(self, node_start, node_end):
dist, theta = self.get_distance_and_angle(node_start, node_end)
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 extract_path(self, node_end):
path = [(self.s_goal.x, self.s_goal.y)]
node_now = node_end
while node_now.parent is not None:
node_now = node_now.parent
path.append((node_now.x, node_now.y))
return path
def extract_waypoint(self, node_end):
waypoint = [self.s_goal]
node_now = node_end
while node_now.parent is not None:
node_now = node_now.parent
waypoint.append(node_now)
return waypoint
@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 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.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")
def plot_visited(self, animation=True):
if animation:
count = 0
for node in self.vertex:
count += 1
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])
if count % 10 == 0:
plt.pause(0.001)
else:
for node in self.vertex:
if node.parent:
plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
def plot_vertex_old(self):
for node in self.vertex_old:
if node.parent:
plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
def plot_vertex_new(self):
count = 0
for node in self.vertex_new:
count += 1
if node.parent:
plt.plot([node.parent.x, node.x], [node.parent.y, node.y], color='darkorange')
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)
@staticmethod
def plot_path(path, color='red'):
plt.plot([x[0] for x in path], [x[1] for x in path], linewidth=2, color=color)
plt.pause(0.01)
def main():
x_start = (2, 2) # Starting node
x_goal = (49, 24) # Goal node
drrt = DynamicRrt(x_start, x_goal, 0.5, 0.1, 0.6, 5000)
drrt.planning()
if __name__ == '__main__':
main()
================================================
FILE: Sampling_based_Planning/rrt_2D/env.py
================================================
"""
Environment for rrt_2D
@author: huiming zhou
"""
class Env:
def __init__(self):
self.x_range = (0, 50)
self.y_range = (0, 30)
self.obs_boundary = self.obs_boundary()
self.obs_circle = self.obs_circle()
self.obs_rectangle = self.obs_rectangle()
@staticmethod
def obs_boundary():
obs_boundary = [
[0, 0, 1, 30],
[0, 30, 50, 1],
[1, 0, 50, 1],
[50, 1, 1, 30]
]
return obs_boundary
@staticmethod
def obs_rectangle():
obs_rectangle = [
[14, 12, 8, 2],
[18, 22, 8, 3],
[26, 7, 2, 12],
[32, 14, 10, 2]
]
return obs_rectangle
@staticmethod
def obs_circle():
obs_cir = [
[7, 12, 3],
[46, 20, 2],
[15, 5, 2],
[37, 7, 3],
[37, 23, 3]
]
return obs_cir
================================================
FILE: Sampling_based_Planning/rrt_2D/extended_rrt.py
================================================
"""
EXTENDED_RRT_2D
@author: huiming zhou
"""
import os
import sys
import math
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
class ExtendedRrt:
def __init__(self, s_start, s_goal, step_len, goal_sample_rate, waypoint_sample_rate, iter_max):
self.s_start = Node(s_start)
self.s_goal = Node(s_goal)
self.step_len = step_len
self.goal_sample_rate = goal_sample_rate
self.waypoint_sample_rate = waypoint_sample_rate
self.iter_max = iter_max
self.vertex = [self.s_start]
self.env = env.Env()
self.plotting = plotting.Plotting(s_start, s_goal)
self.utils = utils.Utils()
self.fig, self.ax = plt.subplots()
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.path = []
self.waypoint = []
def planning(self):
for i in range(self.iter_max):
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 node_new and not self.utils.is_collision(node_near, node_new):
self.vertex.append(node_new)
dist, _ = self.get_distance_and_angle(node_new, self.s_goal)
if dist <= self.step_len:
self.new_state(node_new, self.s_goal)
path = self.extract_path(node_new)
self.plot_grid("Extended_RRT")
self.plot_visited()
self.plot_path(path)
self.path = path
self.waypoint = self.extract_waypoint(node_new)
self.fig.canvas.mpl_connect('button_press_event', self.on_press)
plt.show()
return
return None
def on_press(self, event):
x, y = event.xdata, event.ydata
if x < 0 or x > 50 or y < 0 or y > 30:
print("Please choose right area!")
else:
x, y = int(x), int(y)
print("Add circle obstacle at: s =", x, ",", "y =", y)
self.obs_circle.append([x, y, 2])
self.utils.update_obs(self.obs_circle, self.obs_boundary, self.obs_rectangle)
path, waypoint = self.replanning()
plt.cla()
self.plot_grid("Extended_RRT")
self.plot_path(self.path, color='blue')
self.plot_visited()
self.plot_path(path)
self.path = path
self.waypoint = waypoint
self.fig.canvas.draw_idle()
def replanning(self):
self.vertex = [self.s_start]
for i in range(self.iter_max):
node_rand = self.generate_random_node_replanning(self.goal_sample_rate, self.waypoint_sample_rate)
node_near = self.nearest_neighbor(self.vertex, node_rand)
node_new = self.new_state(node_near, node_rand)
if node_new and not self.utils.is_collision(node_near, node_new):
self.vertex.append(node_new)
dist, _ = self.get_distance_and_angle(node_new, self.s_goal)
if dist <= self.step_len:
self.new_state(node_new, self.s_goal)
path = self.extract_path(node_new)
waypoint = self.extract_waypoint(node_new)
return path, waypoint
return None
def generate_random_node(self, goal_sample_rate):
delta = self.utils.delta
if np.random.random() > goal_sample_rate:
return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
return self.s_goal
def generate_random_node_replanning(self, goal_sample_rate, waypoint_sample_rate):
delta = self.utils.delta
p = np.random.random()
if p < goal_sample_rate:
return self.s_goal
elif goal_sample_rate < p < goal_sample_rate + waypoint_sample_rate:
return self.waypoint[np.random.randint(0, len(self.path) - 1)]
else:
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)))
@staticmethod
def nearest_neighbor(node_list, n):
return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
for nd in node_list]))]
def new_state(self, node_start, node_end):
dist, theta = self.get_distance_and_angle(node_start, node_end)
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 extract_path(self, node_end):
path = [(self.s_goal.x, self.s_goal.y)]
node_now = node_end
while node_now.parent is not None:
node_now = node_now.parent
path.append((node_now.x, node_now.y))
return path
def extract_waypoint(self, node_end):
waypoint = [self.s_goal]
node_now = node_end
while node_now.parent is not None:
node_now = node_now.parent
waypoint.append(node_now)
return waypoint
@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 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.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")
def plot_visited(self):
animation = True
if animation:
count = 0
for node in self.vertex:
count += 1
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])
if count % 10 == 0:
plt.pause(0.001)
else:
for node in self.vertex:
if node.parent:
plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
@staticmethod
def plot_path(path, color='red'):
plt.plot([x[0] for x in path], [x[1] for x in path], linewidth=2, color=color)
plt.pause(0.01)
def main():
x_start = (2, 2) # Starting node
x_goal = (49, 24) # Goal node
errt = ExtendedRrt(x_start, x_goal, 0.5, 0.1, 0.6, 5000)
errt.planning()
if __name__ == '__main__':
main()
================================================
FILE: Sampling_based_Planning/rrt_2D/fast_marching_trees.py
================================================
"""
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()
================================================
FILE: Sampling_based_Planning/rrt_2D/informed_rrt_star.py
================================================
"""
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()
================================================
FILE: Sampling_based_Planning/rrt_2D/plotting.py
================================================
"""
Plotting tools for Sampling-based algorithms
@author: huiming zhou
"""
import matplotlib.pyplot as plt
import matplotlib.patches as patches
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Sampling_based_Planning/")
from Sampling_based_Planning.rrt_2D import env
class Plotting:
def __init__(self, x_start, x_goal):
self.xI, self.xG = x_start, x_goal
self.env = env.Env()
self.obs_bound = self.env.obs_boundary
self.obs_circle = self.env.obs_circle
self.obs_rectangle = self.env.obs_rectangle
def animation(self, nodelist, path, name, animation=False):
self.plot_grid(name)
self.plot_visited(nodelist, animation)
self.plot_path(path)
def animation_connect(self, V1, V2, path, name):
self.plot_grid(name)
self.plot_visited_connect(V1, V2)
self.plot_path(path)
def plot_grid(self, name):
fig, ax = plt.subplots()
for (ox, oy, w, h) in self.obs_bound:
ax.add_patch(
patches.Rectangle(
(ox, oy), w, h,
edgecolor='black',
facecolor='black',
fill=True
)
)
for (ox, oy, w, h) in self.obs_rectangle:
ax.add_patch(
patches.Rectangle(
(ox, oy), w, h,
edgecolor='black',
facecolor='gray',
fill=True
)
)
for (ox, oy, r) in self.obs_circle:
ax.add_patch(
patches.Circle(
(ox, oy), r,
edgecolor='black',
facecolor='gray',
fill=True
)
)
plt.plot(self.xI[0], self.xI[1], "bs", linewidth=3)
plt.plot(self.xG[0], self.xG[1], "gs", linewidth=3)
plt.title(name)
plt.axis("equal")
@staticmethod
def plot_visited(nodelist, animation):
if animation:
count = 0
for node in nodelist:
count += 1
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])
if count % 10 == 0:
plt.pause(0.001)
else:
for node in nodelist:
if node.parent:
plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
@staticmethod
def plot_visited_connect(V1, V2):
len1, len2 = len(V1), len(V2)
for k in range(max(len1, len2)):
if k < len1:
if V1[k].parent:
plt.plot([V1[k].x, V1[k].parent.x], [V1[k].y, V1[k].parent.y], "-g")
if k < len2:
if V2[k].parent:
plt.plot([V2[k].x, V2[k].parent.x], [V2[k].y, V2[k].parent.y], "-g")
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if k % 2 == 0:
plt.pause(0.001)
plt.pause(0.01)
@staticmethod
def plot_path(path):
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()
================================================
FILE: Sampling_based_Planning/rrt_2D/queue.py
================================================
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 s using priority
def get(self):
return heapq.heappop(self.queue)[1] # pop out the smallest item
def enumerate(self):
return self.queue
================================================
FILE: Sampling_based_Planning/rrt_2D/rrt.py
================================================
"""
RRT_2D
@author: huiming zhou
"""
import os
import sys
import math
import numpy as np
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 Rrt:
def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max):
self.s_start = Node(s_start)
self.s_goal = Node(s_goal)
self.step_len = step_len
self.goal_sample_rate = goal_sample_rate
self.iter_max = iter_max
self.vertex = [self.s_start]
self.env = env.Env()
self.plotting = plotting.Plotting(s_start, s_goal)
self.utils = utils.Utils()
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
def planning(self):
for i in range(self.iter_max):
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 node_new and not self.utils.is_collision(node_near, node_new):
self.vertex.append(node_new)
dist, _ = self.get_distance_and_angle(node_new, self.s_goal)
if dist <= self.step_len and not self.utils.is_collision(node_new, self.s_goal):
self.new_state(node_new, self.s_goal)
return self.extract_path(node_new)
return None
def generate_random_node(self, goal_sample_rate):
delta = self.utils.delta
if np.random.random() > goal_sample_rate:
return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
return self.s_goal
@staticmethod
def nearest_neighbor(node_list, n):
return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
for nd in node_list]))]
def new_state(self, node_start, node_end):
dist, theta = self.get_distance_and_angle(node_start, node_end)
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 extract_path(self, node_end):
path = [(self.s_goal.x, self.s_goal.y)]
node_now = node_end
while node_now.parent is not None:
node_now = node_now.parent
path.append((node_now.x, node_now.y))
return path
@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 main():
x_start = (2, 2) # Starting node
x_goal = (49, 24) # Goal node
rrt = Rrt(x_start, x_goal, 0.5, 0.05, 10000)
path = rrt.planning()
if path:
rrt.plotting.animation(rrt.vertex, path, "RRT", True)
else:
print("No Path Found!")
if __name__ == '__main__':
main()
================================================
FILE: Sampling_based_Planning/rrt_2D/rrt_connect.py
================================================
"""
RRT_CONNECT_2D
@author: huiming zhou
"""
import os
import sys
import math
import copy
import numpy as np
import matplotlib.pyplot as plt
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 RrtConnect:
def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max):
self.s_start = Node(s_start)
self.s_goal = Node(s_goal)
self.step_len = step_len
self.goal_sample_rate = goal_sample_rate
self.iter_max = iter_max
self.V1 = [self.s_start]
self.V2 = [self.s_goal]
self.env = env.Env()
self.plotting = plotting.Plotting(s_start, s_goal)
self.utils = utils.Utils()
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
def planning(self):
for i in range(self.iter_max):
node_rand = self.generate_random_node(self.s_goal, self.goal_sample_rate)
node_near = self.nearest_neighbor(self.V1, node_rand)
node_new = self.new_state(node_near, node_rand)
if node_new and not self.utils.is_collision(node_near, node_new):
self.V1.append(node_new)
node_near_prim = self.nearest_neighbor(self.V2, node_new)
node_new_prim = self.new_state(node_near_prim, node_new)
if node_new_prim and not self.utils.is_collision(node_new_prim, node_near_prim):
self.V2.append(node_new_prim)
while True:
node_new_prim2 = self.new_state(node_new_prim, node_new)
if node_new_prim2 and not self.utils.is_collision(node_new_prim2, node_new_prim):
self.V2.append(node_new_prim2)
node_new_prim = self.change_node(node_new_prim, node_new_prim2)
else:
break
if self.is_node_same(node_new_prim, node_new):
break
if self.is_node_same(node_new_prim, node_new):
return self.extract_path(node_new, node_new_prim)
if len(self.V2) < len(self.V1):
list_mid = self.V2
self.V2 = self.V1
self.V1 = list_mid
return None
@staticmethod
def change_node(node_new_prim, node_new_prim2):
node_new = Node((node_new_prim2.x, node_new_prim2.y))
node_new.parent = node_new_prim
return node_new
@staticmethod
def is_node_same(node_new_prim, node_new):
if node_new_prim.x == node_new.x and \
node_new_prim.y == node_new.y:
return True
return False
def generate_random_node(self, sample_goal, goal_sample_rate):
delta = self.utils.delta
if np.random.random() > goal_sample_rate:
return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
return sample_goal
@staticmethod
def nearest_neighbor(node_list, n):
return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
for nd in node_list]))]
def new_state(self, node_start, node_end):
dist, theta = self.get_distance_and_angle(node_start, node_end)
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
@staticmethod
def extract_path(node_new, node_new_prim):
path1 = [(node_new.x, node_new.y)]
node_now = node_new
while node_now.parent is not None:
node_now = node_now.parent
path1.append((node_now.x, node_now.y))
path2 = [(node_new_prim.x, node_new_prim.y)]
node_now = node_new_prim
while node_now.parent is not None:
node_now = node_now.parent
path2.append((node_now.x, node_now.y))
return list(list(reversed(path1)) + path2)
@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 main():
x_start = (2, 2) # Starting node
x_goal = (49, 24) # Goal node
rrt_conn = RrtConnect(x_start, x_goal, 0.8, 0.05, 5000)
path = rrt_conn.planning()
rrt_conn.plotting.animation_connect(rrt_conn.V1, rrt_conn.V2, path, "RRT_CONNECT")
if __name__ == '__main__':
main()
================================================
FILE: Sampling_based_Planning/rrt_2D/rrt_sharp.py
================================================
================================================
FILE: Sampling_based_Planning/rrt_2D/rrt_star.py
================================================
"""
RRT_star 2D
@author: huiming zhou
"""
import os
import sys
import math
import numpy as np
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Sampling_based_Planning/")
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.parent = None
class RrtStar:
def __init__(self, x_start, x_goal, step_len,
goal_sample_rate, search_radius, iter_max):
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.s_start]
self.path = []
self.env = env.Env()
self.plotting = plotting.Plotting(x_start, x_goal)
self.utils = utils.Utils()
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
def planning(self):
for k in range(self.iter_max):
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):
neighbor_index = self.find_near_neighbor(node_new)
self.vertex.append(node_new)
if neighbor_index:
self.choose_parent(node_new, neighbor_index)
self.rewire(node_new, neighbor_index)
index = self.search_goal_parent()
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
if np.random.random() > goal_sample_rate:
return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
return self.s_goal
def find_near_neighbor(self, node_new):
n = len(self.vertex) + 1
r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len)
dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex]
dist_table_index = [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
@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]))]
@staticmethod
def cost(node_p):
node = node_p
cost = 0.0
while node.parent:
cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
node = node.parent
return cost
def update_cost(self, parent_node):
OPEN = queue.QueueFIFO()
OPEN.put(parent_node)
while not OPEN.empty():
node = OPEN.get()
if len(node.child) == 0:
continue
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.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
path.append([node.x, node.y])
return path
@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 main():
x_start = (18, 8) # Starting node
x_goal = (37, 18) # Goal node
rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 10000)
rrt_star.planning()
if __name__ == '__main__':
main()
================================================
FILE: Sampling_based_Planning/rrt_2D/rrt_star_smart.py
================================================
"""
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()
================================================
FILE: Sampling_based_Planning/rrt_2D/utils.py
================================================
"""
utils for collision check
@author: huiming zhou
"""
import math
import numpy as np
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Sampling_based_Planning/")
from Sampling_based_Planning.rrt_2D import env
from Sampling_based_Planning.rrt_2D.rrt import Node
class Utils:
def __init__(self):
self.env = env.Env()
self.delta = 0.5
self.obs_circle = self.env.obs_circle
self.obs_rectangle = self.env.obs_rectangle
self.obs_boundary = self.env.obs_boundary
def update_obs(self, obs_cir, obs_bound, obs_rec):
self.obs_circle = obs_cir
self.obs_boundary = obs_bound
self.obs_rectangle = obs_rec
def get_obs_vertex(self):
delta = self.delta
obs_list = []
for (ox, oy, w, h) in self.obs_rectangle:
vertex_list = [[ox - delta, oy - delta],
[ox + w + delta, oy - delta],
[ox + w + delta, oy + h + delta],
[ox - delta, oy + h + delta]]
obs_list.append(vertex_list)
return obs_list
def is_intersect_rec(self, start, end, o, d, a, b):
v1 = [o[0] - a[0], o[1] - a[1]]
v2 = [b[0] - a[0], b[1] - a[1]]
v3 = [-d[1], d[0]]
div = np.dot(v2, v3)
if div == 0:
return False
t1 = np.linalg.norm(np.cross(v2, v1)) / div
t2 = np.dot(v1, v3) / div
if t1 >= 0 and 0 <= t2 <= 1:
shot = Node((o[0] + t1 * d[0], o[1] + t1 * d[1]))
dist_obs = self.get_dist(start, shot)
dist_seg = self.get_dist(start, end)
if dist_obs <= dist_seg:
return True
return False
def is_intersect_circle(self, o, d, a, r):
d2 = np.dot(d, d)
delta = self.delta
if d2 == 0:
return False
t = np.dot([a[0] - o[0], a[1] - o[1]], d) / d2
if 0 <= t <= 1:
shot = Node((o[0] + t * d[0], o[1] + t * d[1]))
if self.get_dist(shot, Node(a)) <= r + delta:
return True
return False
def is_collision(self, start, end):
if self.is_inside_obs(start) or self.is_inside_obs(end):
return True
o, d = self.get_ray(start, end)
obs_vertex = self.get_obs_vertex()
for (v1, v2, v3, v4) in obs_vertex:
if self.is_intersect_rec(start, end, o, d, v1, v2):
return True
if self.is_intersect_rec(start, end, o, d, v2, v3):
return True
if self.is_intersect_rec(start, end, o, d, v3, v4):
return True
if self.is_intersect_rec(start, end, o, d, v4, v1):
return True
for (x, y, r) in self.obs_circle:
if self.is_intersect_circle(o, d, [x, y], r):
return True
return False
def is_inside_obs(self, node):
delta = self.delta
for (x, y, r) in self.obs_circle:
if math.hypot(node.x - x, node.y - y) <= r + delta:
return True
for (x, y, w, h) in self.obs_rectangle:
if 0 <= node.x - (x - delta) <= w + 2 * delta \
and 0 <= node.y - (y - delta) <= h + 2 * delta:
return True
for (x, y, w, h) in self.obs_boundary:
if 0 <= node.x - (x - delta) <= w + 2 * delta \
and 0 <= node.y - (y - delta) <= h + 2 * delta:
return True
return False
@staticmethod
def get_ray(start, end):
orig = [start.x, start.y]
direc = [end.x - start.x, end.y - start.y]
return orig, direc
@staticmethod
def get_dist(start, end):
return math.hypot(end.x - start.x, end.y - start.y)
================================================
FILE: Sampling_based_Planning/rrt_3D/ABIT_star3D.py
================================================
# This is Advanced Batched Informed Tree star 3D algorithm
# implementation
"""
This is ABIT* code for 3D
@author: yue qi
source: M.P.Strub, J.D.Gammel. "Advanced BIT* (ABIT*):
Sampling-Based Planning with Advanced Graph-Search Techniques"
"""
import numpy as np
import matplotlib.pyplot as plt
import time
import copy
import os
import sys
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
from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
from rrt_3D.queue import MinheapPQ
class ABIT_star:
def __init__(self):
self.env = env()
self.xstart, self.xgoal = tuple(self.env.start), tuple(self.env.goal)
self.maxiter = 1000
self.done = False
self.n = 1000# used in radius calc r(q)
self.lam = 10 # used in radius calc r(q)
def run(self):
V, E = {self.xstart}, set()
T = (V,E)
Xunconnected = {self.xgoal}
q = len(V) + len(Xunconnected)
eps_infl, eps_trunc = np.inf, np.inf
Vclosed, Vinconsistent = set(), set()
Q = self.expand(self.xstart, T, Xunconnected, np.inf)
ind = 0
while True:
if self.is_search_marked_finished():
if self.update_approximation(eps_infl, eps_trunc):
T, Xunconnected = self.prune(T, Xunconnected, self.xgoal)
Xunconnected.update(self.sample(m, self.xgoal))
q = len(V) + len(Xunconnected)
Q = self.expand({self.xstart}, T, Xunconnected, self.r(q))
else:
Q.update(self.expand(Vinconsistent, T, Xunconnected, self.r(q)))
eps_infl = self.update_inflation_factor()
eps_trunc = self.update_truncation_factor()
Vclosed = set()
Vinconsistent = set()
self.mark_search_unfinished()
else:
state_tuple = list(Q)
(xp, xc) = state_tuple[np.argmin( [self.g_T[xi] + self.c_hat(xi,xj) + eps_infl * self.h_hat(xj) for (xi,xj) in Q] )]
Q = Q.difference({(xp, xc)})
if (xp, xc) in E:
if xc in Vclosed:
Vinconsistent.add(xc)
else:
Q.update(self.expand({xc}, T, Xunconnected, self.r(q)))
Vclosed.add(xc)
elif eps_trunc * (self.g_T(xp) + self.c_hat(xi, xj) + self.h_hat(xc)) <= self.g_T(self.xgoal):
if self.g_T(xp) + self.c_hat(xp, xc) < self.g_T(xc):
if self.g_T(xp) + self.c(xp, xc) + self.h_hat(xc) < self.g_T(self.xgoal):
if self.g_T(xp) + self.c(xp, xc) < self.g_T(xc):
if xc in V:
E = E.difference({(xprev, xc)})
else:
Xunconnected.difference_update({xc})
V.add(xc)
E.add((xp, xc))
if xc in Vclosed:
Vinconsistent.add(xc)
else:
Q.update(self.expand({xc}, T, Xunconnected, self.r(q)))
Vclosed.add(xc)
else:
self.mark_search_finished()
ind += 1
# until stop
if ind > self.maxiter:
break
def sample(self, m, xgoal):
pass
def expand(self, set_xi, T, Xunconnected, r):
V, E = T
Eout = set()
for xp in set_xi:
Eout.update({(x1, x2) for (x1, x2) in E if x1 == xp})
for xc in {x for x in Xunconnected.union(V) if getDist(xp, x) <= r}:
if self.g_hat(xp) + self.c_hat(xp, xc) + self.h_hat(xc) <= self.g_T(self.xgoal):
if self.g_hat(xp) + self.c_hat(xp, xc) <= self.g_hat(xc):
Eout.add((xp,xc))
return Eout
def prune(self, T, Xunconnected, xgoal):
V, E = T
Xunconnected.difference_update({x for x in Xunconnected if self.f_hat(x) >= self.g_T(xgoal)})
V.difference_update({x for x in V if self.f_hat(x) > self.g_T(xgoal)})
E.difference_update({(xp, xc) for (xp, xc) in E if self.f_hat(xp) > self.g_T(xgoal) or self.f_hat(xc) > self.g_T(xgoal)})
Xunconnected.update({xc for (xp, xc) in E if (xp not in V) and (xc in V)})
V.difference_update({xc for (xp, xc) in E if (xp not in V) and (xc in V)})
T = (V,E)
return T, Xunconnected
def g_hat(self, x):
pass
def h_hat(self, x):
pass
def c_hat(self, x1, x2):
pass
def f_hat(self, x):
pass
def g_T(self, x):
pass
def r(self, q):
return self.eta * (2 * (1 + 1/self.n) * (self.Lambda(self.Xf_hat) / self.Zeta) * (np.log(q) / q)) ** (1 / self.n)
def Lambda(self, inputset):
pass
def Zeta(self):
pass
def is_search_marked_finished(self):
return self.done
def mark_search_unfinished(self):
self.done = False
return self.done
def mark_search_finished(self):
self.done = True
return self.done
================================================
FILE: Sampling_based_Planning/rrt_3D/BIT_star3D.py
================================================
# This is Batched Informed Tree star 3D algorithm
# implementation
"""
This is ABIT* code for 3D
@author: yue qi
Algorithm 1
source: Gammell, Jonathan D., Siddhartha S. Srinivasa, and Timothy D. Barfoot. "Batch informed trees (BIT*):
Sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs."
2015 IEEE international conference on robotics and automation (ICRA). IEEE, 2015.
and
source: Gammell, Jonathan D., Timothy D. Barfoot, and Siddhartha S. Srinivasa.
"Batch Informed Trees (BIT*): Informed asymptotically optimal anytime search."
The International Journal of Robotics Research 39.5 (2020): 543-567.
"""
import numpy as np
import matplotlib.pyplot as plt
from numpy.matlib import repmat
import time
import copy
import os
import sys
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, isinside, isinbound
from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
from rrt_3D.queue import MinheapPQ
#---------methods to draw ellipse during sampling
def CreateUnitSphere(r = 1):
phi = np.linspace(0,2*np.pi, 256).reshape(256, 1) # the angle of the projection in the xy-plane
theta = np.linspace(0, np.pi, 256).reshape(-1, 256) # the angle from the polar axis, ie the polar angle
radius = r
# Transformation formulae for a spherical coordinate system.
x = radius*np.sin(theta)*np.cos(phi)
y = radius*np.sin(theta)*np.sin(phi)
z = radius*np.cos(theta)
return (x, y, z)
def draw_ellipsoid(ax, C, L, xcenter):
(xs, ys, zs) = CreateUnitSphere()
pts = np.array([xs, ys, zs])
pts_in_world_frame = C@L@pts + xcenter
ax.plot_surface(pts_in_world_frame[0], pts_in_world_frame[1], pts_in_world_frame[2], alpha=0.05, color="g")
class BIT_star:
# ---------initialize and run
def __init__(self, show_ellipse=False):
self.env = env()
self.xstart, self.xgoal = tuple(self.env.start), tuple(self.env.goal)
self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
self.maxiter = 1000 # used for determining how many batches needed
# radius calc parameter:
# larger value makes better 1-time-performance, but longer time trade off
self.eta = 7 # bigger or equal to 1
# sampling
self.m = 400 # number of samples for one time sample
self.d = 3 # dimension we work with
# instance of the cost to come gT
self.g = {self.xstart:0, self.xgoal:np.inf}
# draw ellipse
self.show_ellipse = show_ellipse
# denote if the path is found
self.done = False
self.Path = []
# for drawing the ellipse
self.C = np.zeros([3,3])
self.L = np.zeros([3,3])
self.xcenter = np.zeros(3)
self.show_ellipse = show_ellipse
def run(self):
self.V = {self.xstart} # node expanded
self.E = set() # edge set
self.Parent = {} # Parent relation
# self.T = (self.V, self.E) # tree
self.Xsamples = {self.xgoal} # sampled set
self.QE = set() # edges in queue
self.QV = set() # nodes in queue
self.r = np.inf # radius for evaluation
self.ind = 0
num_resample = 0
while True:
# for the first round
print('round '+str(self.ind))
self.visualization()
# print(len(self.V))
if len(self.QE) == 0 and len(self.QV) == 0:
self.Prune(self.g_T(self.xgoal))
self.Xsamples = self.Sample(self.m, self.g_T(self.xgoal)) # sample function
self.Xsamples.add(self.xgoal) # adding goal into the sample
self.Vold = {v for v in self.V}
self.QV = {v for v in self.V}
# setting the radius
if self.done:
self.r = 2 # sometimes the original radius criteria makes the radius too small to improve existing tree
num_resample += 1
else:
self.r = self.radius(len(self.V) + len(self.Xsamples)) # radius determined with the sample size and dimension of conf space
while self.BestQueueValue(self.QV, mode = 'QV') <= self.BestQueueValue(self.QE, mode = 'QE'):
self.ExpandVertex(self.BestInQueue(self.QV, mode = 'QV'))
(vm, xm) = self.BestInQueue(self.QE, mode = 'QE')
self.QE.remove((vm, xm))
if self.g_T(vm) + self.c_hat(vm, xm) + self.h_hat(xm) < self.g_T(self.xgoal):
cost = self.c(vm, xm)
if self.g_hat(vm) + cost + self.h_hat(xm) < self.g_T(self.xgoal):
if self.g_T(vm) + cost < self.g_T(xm):
if xm in self.V:
self.E.difference_update({(v, x) for (v, x) in self.E if x == xm})
else:
self.Xsamples.remove(xm)
self.V.add(xm)
self.QV.add(xm)
self.g[xm] = self.g[vm] + cost
self.E.add((vm, xm))
self.Parent[xm] = vm # add parent or update parent
self.QE.difference_update({(v, x) for (v, x) in self.QE if x == xm and (self.g_T(v) + self.c_hat(v, xm)) >= self.g_T(xm)})
# reinitializing sampling
else:
self.QE = set()
self.QV = set()
self.ind += 1
# if the goal is reached
if self.xgoal in self.Parent:
print('locating path...')
self.done = True
self.Path = self.path()
# if the iteration is bigger
if self.ind > self.maxiter:
break
print('complete')
print('number of times resampling ' + str(num_resample))
# ---------IRRT utils
def Sample(self, m, cmax, bias = 0.05, xrand = set()):
# sample within a eclipse
print('new sample')
if cmax < np.inf:
cmin = getDist(self.xgoal, self.xstart)
xcenter = np.array([(self.xgoal[0] + self.xstart[0]) / 2, (self.xgoal[1] + self.xstart[1]) / 2, (self.xgoal[2] + self.xstart[2]) / 2])
C = self.RotationToWorldFrame(self.xstart, self.xgoal)
r = np.zeros(3)
r[0] = cmax /2
for i in range(1,3):
r[i] = np.sqrt(cmax**2 - cmin**2) / 2
L = np.diag(r) # R3*3
xball = self.SampleUnitBall(m) # np.array
x = (C@L@xball).T + repmat(xcenter, len(xball.T), 1)
# x2 = set(map(tuple, x))
self.C = C # save to global var
self.xcenter = xcenter
self.L = L
x2 = set(map(tuple, x[np.array([not isinside(self, state) and isinbound(self.env.boundary, state) for state in x])])) # intersection with the state space
xrand.update(x2)
# if there are samples inside obstacle: recursion
if len(x2) < m:
return self.Sample(m - len(x2), cmax, bias=bias, xrand=xrand)
else:
for i in range(m):
xrand.add(tuple(sampleFree(self, bias = bias)))
return xrand
def SampleUnitBall(self, n):
# uniform sampling in spherical coordinate system in 3D
# sample radius
r = np.random.uniform(0.0, 1.0, size = n)
theta = np.random.uniform(0, np.pi, size = n)
phi = np.random.uniform(0, 2 * np.pi, size = n)
x = r * np.sin(theta) * np.cos(phi)
y = r * np.sin(theta) * np.sin(phi)
z = r * np.cos(theta)
return np.array([x,y,z])
def RotationToWorldFrame(self, xstart, xgoal):
# S0(n): such that the xstart and xgoal are the center points
d = getDist(xstart, xgoal)
xstart, xgoal = np.array(xstart), np.array(xgoal)
a1 = (xgoal - xstart) / d
M = np.outer(a1,[1,0,0])
U, S, V = np.linalg.svd(M)
C = U@np.diag([1, 1, np.linalg.det(U)*np.linalg.det(V)])@V.T
return C
#----------BIT_star particular
def ExpandVertex(self, v):
self.QV.remove(v)
Xnear = {x for x in self.Xsamples if getDist(x, v) <= self.r}
self.QE.update({(v, x) for x in Xnear if self.g_hat(v) + self.c_hat(v, x) + self.h_hat(x) < self.g_T(self.xgoal)})
if v not in self.Vold:
Vnear = {w for w in self.V if getDist(w, v) <= self.r}
self.QE.update({(v,w) for w in Vnear if \
((v,w) not in self.E) and \
(self.g_hat(v) + self.c_hat(v, w) + self.h_hat(w) < self.g_T(self.xgoal)) and \
(self.g_T(v) + self.c_hat(v, w) < self.g_T(w))})
def Prune(self, c):
self.Xsamples = {x for x in self.Xsamples if self.f_hat(x) >= c}
self.V.difference_update({v for v in self.V if self.f_hat(v) >= c})
self.E.difference_update({(v, w) for (v, w) in self.E if (self.f_hat(v) > c) or (self.f_hat(w) > c)})
self.Xsamples.update({v for v in self.V if self.g_T(v) == np.inf})
self.V.difference_update({v for v in self.V if self.g_T(v) == np.inf})
def radius(self, q):
return 2 * self.eta * (1 + 1/self.d) ** (1/self.d) * \
(self.Lambda(self.Xf_hat(self.V)) / self.Zeta() ) ** (1/self.d) * \
(np.log(q) / q) ** (1/self.d)
def Lambda(self, inputset):
# lebesgue measure of a set, defined as
# mu: L(Rn) --> [0, inf], e.g. volume
return len(inputset)
def Xf_hat(self, X):
# the X is a set, defined as {x in X | fhat(x) <= cbest}
# where cbest is current best cost.
cbest = self.g_T(self.xgoal)
return {x for x in X if self.f_hat(x) <= cbest}
def Zeta(self):
# Lebesgue measure of a n dimensional unit ball
# since it's the 3D, use volume
return 4/3 * np.pi
def BestInQueue(self, inputset, mode):
# returns the best vertex in the vertex queue given this ordering
# mode = 'QE' or 'QV'
if mode == 'QV':
V = {state: self.g_T(state) + self.h_hat(state) for state in self.QV}
if mode == 'QE':
V = {state: self.g_T(state[0]) + self.c_hat(state[0], state[1]) + self.h_hat(state[1]) for state in self.QE}
if len(V) == 0:
print(mode + 'empty')
return None
return min(V, key = V.get)
def BestQueueValue(self, inputset, mode):
# returns the best value in the vertex queue given this ordering
# mode = 'QE' or 'QV'
if mode == 'QV':
V = {self.g_T(state) + self.h_hat(state) for state in self.QV}
if mode == 'QE':
V = {self.g_T(state[0]) + self.c_hat(state[0], state[1]) + self.h_hat(state[1]) for state in self.QE}
if len(V) == 0:
return np.inf
return min(V)
def g_hat(self, v):
return getDist(self.xstart, v)
def h_hat(self, v):
return getDist(self.xgoal, v)
def f_hat(self, v):
# f = g + h: estimate cost
return self.g_hat(v) + self.h_hat(v)
def c(self, v, w):
# admissible estimate of the cost of an edge between state v, w
collide, dist = isCollide(self, v, w)
if collide:
return np.inf
else:
return dist
def c_hat(self, v, w):
# c_hat < c < np.inf
# heuristic estimate of the edge cost, since c is expensive
return getDist(v, w)
def g_T(self, v):
# represent cost-to-come from the start in the tree,
# if the state is not in tree, or unreachable, return inf
if v not in self.g:
self.g[v] = np.inf
return self.g[v]
def path(self):
path = []
s = self.xgoal
i = 0
while s != self.xstart:
path.append((s, self.Parent[s]))
s = self.Parent[s]
if i > self.m:
break
i += 1
return path
def visualization(self):
if self.ind % 20 == 0:
V = np.array(list(self.V))
Xsample = np.array(list(self.Xsamples))
edges = list(map(list, self.E))
Path = np.array(self.Path)
start = self.env.start
goal = self.env.goal
# edges = E.get_edge()
#----------- list structure
# edges = []
# for i in self.Parent:
# edges.append([i,self.Parent[i]])
#----------- end
# generate axis objects
ax = plt.subplot(111, projection='3d')
# ax.view_init(elev=0.+ 0.03*self.ind/(2*np.pi), azim=90 + 0.03*self.ind/(2*np.pi))
# ax.view_init(elev=0., azim=90.)
ax.view_init(elev=90., azim=60.)
# ax.view_init(elev=-8., azim=180)
ax.clear()
# drawing objects
draw_Spheres(ax, self.env.balls)
draw_block_list(ax, self.env.blocks)
if self.env.OBB is not None:
draw_obb(ax, self.env.OBB)
draw_block_list(ax, np.array([self.env.boundary]), alpha=0)
draw_line(ax, edges, visibility=0.75, color='g')
draw_line(ax, Path, color='r')
if self.show_ellipse:
draw_ellipsoid(ax, self.C, self.L, self.xcenter) # beware, depending on start and goal position, this might be bad for vis
if len(V) > 0:
ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', )
if len(Xsample) > 0: # plot the sampled points
ax.scatter3D(Xsample[:, 0], Xsample[:, 1], Xsample[:, 2], s=1, color='b',)
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
ax.dist = 11
set_axes_equal(ax)
make_transparent(ax)
#plt.xlabel('s')
#plt.ylabel('y')
ax.set_axis_off()
plt.pause(0.0001)
if __name__ == '__main__':
Newprocess = BIT_star(show_ellipse=False)
Newprocess.run()
================================================
FILE: Sampling_based_Planning/rrt_3D/FMT_star3D.py
================================================
"""
This is fast marching tree* code for 3D
@author: yue qi
source: Janson, Lucas, et al. "Fast marching tree: A fast marching sampling-based method
for optimal motion planning in many dimensions."
The International journal of robotics research 34.7 (2015): 883-921.
"""
import numpy as np
import matplotlib.pyplot as plt
import time
import copy
import os
import sys
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
from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
from rrt_3D.queue import MinheapPQ
class FMT_star:
def __init__(self, radius = 1, n = 1000):
self.env = env()
# init start and goal
# note that the xgoal could be a region since this algorithm is a multiquery method
self.xinit, self.xgoal = tuple(self.env.start), tuple(self.env.goal)
self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) # used for sample free
self.n = n # number of samples
self.radius = radius # radius of the ball
# self.radius = 40 * np.sqrt((np.log(self.n) / self.n))
# sets
self.Vopen, self.Vopen_queue, self.Vclosed, self.V, self.Vunvisited, self.c = self.initNodeSets()
# make space for save
self.neighbors = {}
# additional
self.done = True
self.Path = []
self.Parent = {}
def generateSampleSet(self, n):
V = set()
for i in range(n):
V.add(tuple(sampleFree(self, bias = 0.0)))
return V
def initNodeSets(self):
# open set
Vopen = {self.xinit} # open set
# closed set
closed = set()
# V, Vunvisited set
V = self.generateSampleSet(self.n - 2) # set of all nodes
Vunvisited = copy.deepcopy(V) # unvisited set
Vunvisited.add(self.xgoal)
V.add(self.xinit)
V.add(self.xgoal)
# initialize all cost to come at inf
c = {node : np.inf for node in V}
c[self.xinit] = 0
# use a min heap to speed up
Vopen_queue = MinheapPQ()
Vopen_queue.put(self.xinit, c[self.xinit]) # priority organized as the cost to come
return Vopen, Vopen_queue, closed, V, Vunvisited, c
def Near(self, nodeset, node, rn):
if node in self.neighbors:
return self.neighbors[node]
validnodes = {i for i in nodeset if getDist(i, node) < rn}
return validnodes
def Save(self, V_associated, node):
self.neighbors[node] = V_associated
def path(self, z, initT):
path = []
s = self.xgoal
i = 0
while s != self.xinit:
path.append((s, self.Parent[s]))
s = self.Parent[s]
if i > self.n:
break
i += 1
return path
def Cost(self, x, y):
# collide, dist = isCollide(self, x, y)
# if collide:
# return np.inf
# return dist
return getDist(x, y)
def FMTrun(self):
z = self.xinit
rn = self.radius
Nz = self.Near(self.Vunvisited, z, rn)
E = set()
self.Save(Nz, z)
ind = 0
while z != self.xgoal:
Vopen_new = set()
#Nz = self.Near(self.Vunvisited, z, rn)
#self.Save(Nz, z)
#Xnear = Nz.intersection(self.Vunvisited)
Xnear = self.Near(self.Vunvisited, z ,rn)
self.Save(Xnear, z)
for x in Xnear:
#Nx = self.Near(self.V.difference({x}), x, rn)
#self.Save(Nx, x)
#Ynear = list(Nx.intersection(self.Vopen))
Ynear = list(self.Near(self.Vopen, x, rn))
# self.Save(set(Ynear), x)
ymin = Ynear[np.argmin([self.c[y] + self.Cost(y,x) for y in Ynear])] # DP programming equation
collide, _ = isCollide(self, ymin, x)
if not collide:
E.add((ymin, x)) # straight line joining ymin and x is collision free
Vopen_new.add(x)
self.Parent[x] = z
self.Vunvisited = self.Vunvisited.difference({x})
self.c[x] = self.c[ymin] + self.Cost(ymin, x) # estimated cost-to-arrive from xinit in tree T = (VopenUVclosed, E)
# update open set
self.Vopen = self.Vopen.union(Vopen_new).difference({z})
self.Vclosed.add(z)
if len(self.Vopen) == 0:
print('Failure')
return
ind += 1
print(str(ind) + ' node expanded')
self.visualization(ind, E)
# update current node
Vopenlist = list(self.Vopen)
z = Vopenlist[np.argmin([self.c[y] for y in self.Vopen])]
# creating the tree
T = (self.Vopen.union(self.Vclosed), E)
self.done = True
self.Path = self.path(z, T)
self.visualization(ind, E)
plt.show()
# return self.path(z, T)
def visualization(self, ind, E):
if ind % 100 == 0 or self.done:
#----------- list structure
# V = np.array(list(initparams.V))
# E = initparams.E
#----------- end
# edges = initparams.E
Path = np.array(self.Path)
start = self.env.start
goal = self.env.goal
# edges = E.get_edge()
#----------- list structure
edges = np.array(list(E))
#----------- end
# 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=65., azim=60.)
ax.dist = 15
# ax.view_init(elev=-8., azim=180)
ax.clear()
# drawing objects
draw_Spheres(ax, self.env.balls)
draw_block_list(ax, self.env.blocks)
if self.env.OBB is not None:
draw_obb(ax, self.env.OBB)
draw_block_list(ax, np.array([self.env.boundary]), alpha=0)
draw_line(ax, edges, visibility=0.75, color='g')
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
set_axes_equal(ax)
make_transparent(ax)
#plt.xlabel('x')
#plt.ylabel('y')
ax.set_axis_off()
plt.pause(0.0001)
if __name__ == '__main__':
A = FMT_star(radius = 1, n = 3000)
A.FMTrun()
================================================
FILE: Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py
================================================
"""
This is dynamic rrt code for 3D
@author: yue qi
"""
import numpy as np
import time
import matplotlib.pyplot as plt
import os
import sys
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
from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
class dynamic_rrt_3D:
def __init__(self):
self.env = env()
self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
self.qrobot = self.x0
self.current = tuple(self.env.start)
self.stepsize = 0.25
self.maxiter = 10000
self.GoalProb = 0.05 # probability biased to the goal
self.WayPointProb = 0.02 # probability falls back on to the way points
self.done = False
self.invalid = False
self.V = [] # vertices
self.Parent = {} # parent child relation
self.Edge = set() # edge relation (node, parent node) tuple
self.Path = []
self.flag = {} # flag dictionary
self.ind = 0
self.i = 0
# --------Dynamic RRT algorithm
def RegrowRRT(self):
self.TrimRRT()
self.GrowRRT()
def TrimRRT(self):
S = []
i = 1
print('trimming...')
while i < len(self.V):
qi = self.V[i]
qp = self.Parent[qi]
if self.flag[qp] == 'Invalid':
self.flag[qi] = 'Invalid'
if self.flag[qi] != 'Invalid':
S.append(qi)
i += 1
self.CreateTreeFromNodes(S)
def InvalidateNodes(self, obstacle):
Edges = self.FindAffectedEdges(obstacle)
for edge in Edges:
qe = self.ChildEndpointNode(edge)
self.flag[qe] = 'Invalid'
# --------Extend RRT algorithm-----
def initRRT(self):
self.V.append(self.x0)
self.flag[self.x0] = 'Valid'
def GrowRRT(self):
print('growing...')
qnew = self.x0
distance_threshold = self.stepsize
self.ind = 0
while self.ind <= self.maxiter:
qtarget = self.ChooseTarget()
qnearest = self.Nearest(qtarget)
qnew, collide = self.Extend(qnearest, qtarget)
if not collide:
self.AddNode(qnearest, qnew)
if getDist(qnew, self.xt) < distance_threshold:
self.AddNode(qnearest, self.xt)
self.flag[self.xt] = 'Valid'
break
self.i += 1
self.ind += 1
# self.visualization()
def ChooseTarget(self):
# return the goal, or randomly choose a state in the waypoints based on probs
p = np.random.uniform()
if len(self.V) == 1:
i = 0
else:
i = np.random.randint(0, high=len(self.V) - 1)
if 0 < p < self.GoalProb:
return self.xt
elif self.GoalProb < p < self.GoalProb + self.WayPointProb:
return self.V[i]
elif self.GoalProb + self.WayPointProb < p < 1:
return tuple(self.RandomState())
def RandomState(self):
# generate a random, obstacle free state
xrand = sampleFree(self, bias=0)
return xrand
def AddNode(self, nearest, extended):
self.V.append(extended)
self.Parent[extended] = nearest
self.Edge.add((extended, nearest))
self.flag[extended] = 'Valid'
def Nearest(self, target):
# TODO use kdTree to speed up search
return nearest(self, target, isset=True)
def Extend(self, nearest, target):
extended, dist = steer(self, nearest, target, DIST=True)
collide, _ = isCollide(self, nearest, target, dist)
return extended, collide
# --------Main function
def Main(self):
# qstart = qgoal
self.x0 = tuple(self.env.goal)
# qgoal = qrobot
self.xt = tuple(self.env.start)
self.initRRT()
self.GrowRRT()
self.Path, D = self.path()
self.done = True
self.visualization()
t = 0
while True:
# move the block while the robot is moving
new, _ = self.env.move_block(a=[0.2, 0, -0.2], mode='translation')
self.InvalidateNodes(new)
self.TrimRRT()
# if solution path contains invalid node
self.visualization()
self.invalid = self.PathisInvalid(self.Path)
if self.invalid:
self.done = False
self.RegrowRRT()
self.Path = []
self.Path, D = self.path()
self.done = True
self.visualization()
if t == 8:
break
t += 1
self.visualization()
plt.show()
# --------Additional utility functions
def FindAffectedEdges(self, obstacle):
# scan the graph for the changed edges in the tree.
# return the end point and the affected
print('finding affected edges...')
Affectededges = []
for e in self.Edge:
child, parent = e
collide, _ = isCollide(self, child, parent)
if collide:
Affectededges.append(e)
return Affectededges
def ChildEndpointNode(self, edge):
return edge[0]
def CreateTreeFromNodes(self, Nodes):
print('creating tree...')
# self.Parent = {node: self.Parent[node] for node in Nodes}
self.V = [node for node in Nodes]
self.Edge = {(node, self.Parent[node]) for node in Nodes}
# if self.invalid:
# del self.Parent[self.xt]
def PathisInvalid(self, path):
for edge in path:
if self.flag[tuple(edge[0])] == 'Invalid' or self.flag[tuple(edge[1])] == 'Invalid':
return True
def path(self, dist=0):
Path=[]
x = self.xt
i = 0
while x != self.x0:
x2 = self.Parent[x]
Path.append(np.array([x, x2]))
dist += getDist(x, x2)
x = x2
if i > 10000:
print('Path is not found')
return
i+= 1
return Path, dist
# --------Visualization specialized for dynamic RRT
def visualization(self):
if self.ind % 100 == 0 or self.done:
V = np.array(self.V)
Path = np.array(self.Path)
start = self.env.start
goal = self.env.goal
# edges = []
# for i in self.Parent:
# edges.append([i, self.Parent[i]])
edges = np.array([list(i) for i in self.Edge])
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=90., azim=0.)
ax.clear()
# drawing objects
draw_Spheres(ax, self.env.balls)
draw_block_list(ax, self.env.blocks)
if self.env.OBB is not None:
draw_obb(ax, self.env.OBB)
draw_block_list(ax, np.array([self.env.boundary]), alpha=0)
draw_line(ax, edges, visibility=0.75, color='g')
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
set_axes_equal(ax)
make_transparent(ax)
# plt.xlabel('s')
# plt.ylabel('y')
ax.set_axis_off()
plt.pause(0.0001)
if __name__ == '__main__':
rrt = dynamic_rrt_3D()
rrt.Main()
================================================
FILE: Sampling_based_Planning/rrt_3D/env3D.py
================================================
# 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):
# s 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 = [[4.00e+00, 1.20e+01, 0.00e+00, 5.00e+00, 2.00e+01, 5.00e+00],
[5.5e+00, 1.20e+01, 0.00e+00, 1.00e+01, 1.30e+01, 5.00e+00],
[1.00e+01, 1.20e+01, 0.00e+00, 1.40e+01, 1.30e+01, 5.00e+00],
[1.00e+01, 9.00e+00, 0.00e+00, 2.00e+01, 1.00e+01, 5.00e+00],
[9.00e+00, 6.00e+00, 0.00e+00, 1.00e+01, 1.00e+01, 5.00e+00]]
Obstacles = []
for i in block:
i = np.array(i)
Obstacles.append([j for j in i])
return np.array(Obstacles)
def getballs():
spheres = [[2.0,6.0,2.5,1.0],[14.0,14.0,2.5,2]]
Obstacles = []
for i in spheres:
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
def getAABB2(blocks):
# used in lineAABB
AABB = []
for i in blocks:
AABB.append(aabb(i))
return AABB
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 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]])
class env():
def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=20, zmax=5, resolution=1):
# def __init__(self, xmin=-5, ymin=0, zmin=-5, xmax=10, ymax=5, zmax=10, 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([5.0,7.0,2.5],[0.5,2.0,2.5],R_matrix(135,0,0)),
obb([12.0,4.0,2.5],[0.5,2.0,2.5],R_matrix(45,0,0))])
self.start = np.array([2.0, 2.0, 2.0])
self.goal = np.array([6.0, 16.0, 0.0])
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], block_to_move = 0, mode = 'translation'):
# 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
# (s',t') = (s + tv, t) is uniform transformation
# (s',t') = (s + 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
# (s',t') = (Rx, t)
def move_OBB(self, obb_to_move = 0, theta=[0,0,0], translation=[0,0,0]):
# theta stands for rotational angles around three principle axis in world frame
# translation stands for translation in the world frame
ori = [self.OBB[obb_to_move]]
# move obb position
self.OBB[obb_to_move].P = \
[self.OBB[obb_to_move].P[0] + translation[0],
self.OBB[obb_to_move].P[1] + translation[1],
self.OBB[obb_to_move].P[2] + translation[2]]
# Calculate orientation
self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2])
# generating transformation matrix
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]),[translation[0],translation[1],translation[2],1]])
return self.OBB[obb_to_move], ori[0]
if __name__ == '__main__':
newenv = env()
================================================
FILE: Sampling_based_Planning/rrt_3D/extend_rrt3D.py
================================================
# Real-Time Randomized Path Planning for Robot Navigation∗
"""
This is rrt extend code for 3D
@author: yue qi
"""
import numpy as np
from numpy.matlib import repmat
from collections import defaultdict
import time
import matplotlib.pyplot as plt
import os
import sys
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
# here attempt to use a KD tree for the data structure implementation
import scipy.spatial.kdtree as KDtree
class extend_rrt(object):
def __init__(self):
self.env = env()
self.x0, self.x
gitextract_yrm3dru6/
├── .idea/
│ ├── .gitignore
│ ├── PathPlanning.iml
│ ├── inspectionProfiles/
│ │ └── profiles_settings.xml
│ ├── misc.xml
│ ├── modules.xml
│ └── vcs.xml
├── CurvesGenerator/
│ ├── bezier_path.py
│ ├── bspline_curve.py
│ ├── cubic_spline.py
│ ├── draw.py
│ ├── dubins_path.py
│ ├── quartic_polynomial.py
│ ├── quintic_polynomial.py
│ └── reeds_shepp.py
├── LICENSE
├── README.md
├── Sampling_based_Planning/
│ ├── rrt_2D/
│ │ ├── adaptively_informed_trees.py
│ │ ├── advanced_batch_informed_trees.py
│ │ ├── batch_informed_trees.py
│ │ ├── dubins_rrt_star.py
│ │ ├── dynamic_rrt.py
│ │ ├── env.py
│ │ ├── extended_rrt.py
│ │ ├── fast_marching_trees.py
│ │ ├── informed_rrt_star.py
│ │ ├── plotting.py
│ │ ├── queue.py
│ │ ├── rrt.py
│ │ ├── rrt_connect.py
│ │ ├── rrt_sharp.py
│ │ ├── rrt_star.py
│ │ ├── rrt_star_smart.py
│ │ └── utils.py
│ └── rrt_3D/
│ ├── ABIT_star3D.py
│ ├── BIT_star3D.py
│ ├── FMT_star3D.py
│ ├── dynamic_rrt3D.py
│ ├── env3D.py
│ ├── extend_rrt3D.py
│ ├── informed_rrt_star3D.py
│ ├── plot_util3D.py
│ ├── queue.py
│ ├── rrt3D.py
│ ├── rrt_connect3D.py
│ ├── rrt_star3D.py
│ ├── rrt_star_smart3D.py
│ └── utils3D.py
└── Search_based_Planning/
├── Search_2D/
│ ├── ARAstar.py
│ ├── Anytime_D_star.py
│ ├── Astar.py
│ ├── Best_First.py
│ ├── Bidirectional_a_star.py
│ ├── D_star.py
│ ├── D_star_Lite.py
│ ├── Dijkstra.py
│ ├── LPAstar.py
│ ├── LRTAstar.py
│ ├── RTAAStar.py
│ ├── bfs.py
│ ├── dfs.py
│ ├── env.py
│ ├── plotting.py
│ └── queue.py
└── Search_3D/
├── Anytime_Dstar3D.py
├── Astar3D.py
├── Dstar3D.py
├── DstarLite3D.py
├── LP_Astar3D.py
├── LRT_Astar3D.py
├── RTA_Astar3D.py
├── bidirectional_Astar3D.py
├── env3D.py
├── plot_util3D.py
├── queue.py
└── utils3D.py
SYMBOL INDEX (844 symbols across 63 files)
FILE: CurvesGenerator/bezier_path.py
function calc_4points_bezier_path (line 14) | def calc_4points_bezier_path(sx, sy, syaw, gx, gy, gyaw, offset):
function calc_bezier_path (line 28) | def calc_bezier_path(control_points, n_points=100):
function Comb (line 37) | def Comb(n, i, t):
function bezier (line 41) | def bezier(t, control_points):
function bezier_derivatives_control_points (line 46) | def bezier_derivatives_control_points(control_points, n_derivatives):
function curvature (line 57) | def curvature(dx, dy, ddx, ddy):
function simulation (line 61) | def simulation():
function main (line 99) | def main():
FILE: CurvesGenerator/bspline_curve.py
function approximate_b_spline_path (line 15) | def approximate_b_spline_path(x, y, n_path_points, degree=3):
function interpolate_b_spline_path (line 33) | def interpolate_b_spline_path(x, y, n_path_points, degree=3):
function main (line 42) | def main():
FILE: CurvesGenerator/cubic_spline.py
class Spline (line 17) | class Spline:
method __init__ (line 22) | def __init__(self, x, y):
method calc (line 47) | def calc(self, t):
method calcd (line 67) | def calcd(self, t):
method calcdd (line 84) | def calcdd(self, t):
method __search_index (line 99) | def __search_index(self, x):
method __calc_A (line 105) | def __calc_A(self, h):
method __calc_B (line 123) | def __calc_B(self, h):
class Spline2D (line 135) | class Spline2D:
method __init__ (line 141) | def __init__(self, x, y):
method __calc_s (line 146) | def __calc_s(self, x, y):
method calc_position (line 155) | def calc_position(self, s):
method calc_curvature (line 164) | def calc_curvature(self, s):
method calc_yaw (line 175) | def calc_yaw(self, s):
function calc_spline_course (line 185) | def calc_spline_course(x, y, ds=0.1):
function test_spline2d (line 200) | def test_spline2d():
function test_spline (line 243) | def test_spline():
FILE: CurvesGenerator/draw.py
class Arrow (line 6) | class Arrow:
method __init__ (line 7) | def __init__(self, x, y, theta, L, c):
class Car (line 35) | class Car:
method __init__ (line 36) | def __init__(self, x, y, yaw, w, L):
FILE: CurvesGenerator/dubins_path.py
class PATH (line 13) | class PATH:
method __init__ (line 14) | def __init__(self, L, mode, x, y, yaw):
function pi_2_pi (line 23) | def pi_2_pi(theta):
function mod2pi (line 33) | def mod2pi(theta):
function LSL (line 37) | def LSL(alpha, beta, dist):
function RSR (line 58) | def RSR(alpha, beta, dist):
function LSR (line 79) | def LSR(alpha, beta, dist):
function RSL (line 100) | def RSL(alpha, beta, dist):
function RLR (line 121) | def RLR(alpha, beta, dist):
function LRL (line 140) | def LRL(alpha, beta, dist):
function interpolate (line 159) | def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):
function generate_local_course (line 189) | def generate_local_course(L, lengths, mode, maxc, step_size):
function planning_from_origin (line 249) | def planning_from_origin(gx, gy, gyaw, curv, step_size):
function calc_dubins_path (line 280) | def calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, curv, step_size=0.1):
function main (line 300) | def main():
FILE: CurvesGenerator/quartic_polynomial.py
class QuarticPolynomial (line 8) | class QuarticPolynomial:
method __init__ (line 9) | def __init__(self, x0, v0, a0, v1, a1, T):
method calc_xt (line 22) | def calc_xt(self, t):
method calc_dxt (line 28) | def calc_dxt(self, t):
method calc_ddxt (line 34) | def calc_ddxt(self, t):
method calc_dddxt (line 39) | def calc_dddxt(self, t):
FILE: CurvesGenerator/quintic_polynomial.py
class QuinticPolynomial (line 12) | class QuinticPolynomial:
method __init__ (line 13) | def __init__(self, x0, v0, a0, x1, v1, a1, T):
method calc_xt (line 29) | def calc_xt(self, t):
method calc_dxt (line 35) | def calc_dxt(self, t):
method calc_ddxt (line 41) | def calc_ddxt(self, t):
method calc_dddxt (line 46) | def calc_dddxt(self, t):
class Trajectory (line 52) | class Trajectory:
method __init__ (line 53) | def __init__(self):
function simulation (line 63) | def simulation():
FILE: CurvesGenerator/reeds_shepp.py
class PATH (line 14) | class PATH:
method __init__ (line 15) | def __init__(self, lengths, ctypes, L, x, y, yaw, directions):
function calc_optimal_path (line 25) | def calc_optimal_path(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_S...
function calc_all_paths (line 38) | def calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):
function set_path (line 60) | def set_path(paths, lengths, ctypes):
function LSL (line 82) | def LSL(x, y, phi):
function LSR (line 93) | def LSR(x, y, phi):
function LRL (line 109) | def LRL(x, y, phi):
function SCS (line 123) | def SCS(x, y, phi, paths):
function SLS (line 136) | def SLS(x, y, phi):
function CSC (line 155) | def CSC(x, y, phi, paths):
function CCC (line 191) | def CCC(x, y, phi, paths):
function calc_tauOmega (line 231) | def calc_tauOmega(u, v, xi, eta, phi):
function LRLRn (line 249) | def LRLRn(x, y, phi):
function LRLRp (line 263) | def LRLRp(x, y, phi):
function CCCC (line 278) | def CCCC(x, y, phi, paths):
function LRSR (line 314) | def LRSR(x, y, phi):
function LRSL (line 329) | def LRSL(x, y, phi):
function CCSC (line 345) | def CCSC(x, y, phi, paths):
function LRSLR (line 417) | def LRSLR(x, y, phi):
function CCSCC (line 435) | def CCSCC(x, y, phi, paths):
function generate_local_course (line 455) | def generate_local_course(L, lengths, mode, maxc, step_size):
function interpolate (line 513) | def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):
function generate_path (line 543) | def generate_path(q0, q1, maxc):
function pi_2_pi (line 564) | def pi_2_pi(theta):
function R (line 574) | def R(x, y):
function M (line 584) | def M(theta):
function get_label (line 598) | def get_label(path):
function calc_curvature (line 611) | def calc_curvature(x, y, yaw, directions):
function check_path (line 647) | def check_path(sx, sy, syaw, gx, gy, gyaw, maxc):
function main (line 669) | def main():
FILE: Sampling_based_Planning/rrt_2D/batch_informed_trees.py
class Node (line 21) | class Node:
method __init__ (line 22) | def __init__(self, x, y):
class Tree (line 28) | class Tree:
method __init__ (line 29) | def __init__(self, x_start, x_goal):
class BITStar (line 42) | class BITStar:
method __init__ (line 43) | def __init__(self, x_start, x_goal, eta, iter_max):
method init (line 67) | def init(self):
method planning (line 81) | def planning(self):
method ExtractPath (line 149) | def ExtractPath(self):
method Prune (line 160) | def Prune(self, cBest):
method cost (line 168) | def cost(self, start, end):
method f_estimated (line 174) | def f_estimated(self, node):
method g_estimated (line 177) | def g_estimated(self, node):
method h_estimated (line 180) | def h_estimated(self, node):
method Sample (line 183) | def Sample(self, m, cMax, cMin, xCenter, C):
method SampleEllipsoid (line 189) | def SampleEllipsoid(self, m, cMax, cMin, xCenter, C):
method SampleFreeSpace (line 213) | def SampleFreeSpace(self, m):
method radius (line 229) | def radius(self, q):
method ExpandVertex (line 236) | def ExpandVertex(self, v):
method BestVertexQueueValue (line 256) | def BestVertexQueueValue(self):
method BestEdgeQueueValue (line 262) | def BestEdgeQueueValue(self):
method BestInVertexQueue (line 269) | def BestInVertexQueue(self):
method BestInEdgeQueue (line 278) | def BestInEdgeQueue(self):
method SampleUnitNBall (line 289) | def SampleUnitNBall():
method RotationToWorldFrame (line 296) | def RotationToWorldFrame(x_start, x_goal, L):
method calc_dist (line 307) | def calc_dist(start, end):
method calc_dist_and_angle (line 311) | def calc_dist_and_angle(node_start, node_end):
method animation (line 316) | def animation(self, xCenter, cMax, cMin, theta):
method plot_grid (line 335) | def plot_grid(self, name):
method draw_ellipse (line 373) | def draw_ellipse(x_center, c_best, dist, theta):
function main (line 390) | def main():
FILE: Sampling_based_Planning/rrt_2D/dubins_rrt_star.py
class Node (line 23) | class Node:
method __init__ (line 24) | def __init__(self, x, y, yaw):
class DubinsRRTStar (line 35) | class DubinsRRTStar:
method __init__ (line 36) | def __init__(self, sx, sy, syaw, gx, gy, gyaw, vehicle_radius, step_len,
method planning (line 61) | def planning(self):
method draw_graph (line 90) | def draw_graph(self, rnd=None):
method plot_start_goal_arrow (line 106) | def plot_start_goal_arrow(self):
method generate_final_course (line 110) | def generate_final_course(self, goal_index):
method calc_dist_to_goal (line 121) | def calc_dist_to_goal(self, x, y):
method search_best_goal_node (line 126) | def search_best_goal_node(self):
method rewire (line 146) | def rewire(self, new_node, near_inds):
method choose_parent (line 161) | def choose_parent(self, new_node, near_inds):
method calc_new_cost (line 184) | def calc_new_cost(self, from_node, to_node):
method propagate_cost_to_leaves (line 188) | def propagate_cost_to_leaves(self, parent_node):
method get_distance_and_angle (line 195) | def get_distance_and_angle(node_start, node_end):
method Near (line 200) | def Near(self, nodelist, node):
method Steer (line 209) | def Steer(self, node_start, node_end):
method Sample (line 228) | def Sample(self):
method Nearest (line 239) | def Nearest(nodelist, n):
method is_collision (line 243) | def is_collision(self, node):
method animation (line 254) | def animation(self):
method plot_arrow (line 259) | def plot_arrow(self):
method plot_grid (line 263) | def plot_grid(self, name):
method obs_circle (line 292) | def obs_circle():
function main (line 306) | def main():
FILE: Sampling_based_Planning/rrt_2D/dynamic_rrt.py
class Node (line 20) | class Node:
method __init__ (line 21) | def __init__(self, n):
class Edge (line 28) | class Edge:
method __init__ (line 29) | def __init__(self, n_p, n_c):
class DynamicRrt (line 35) | class DynamicRrt:
method __init__ (line 36) | def __init__(self, s_start, s_goal, step_len, goal_sample_rate, waypoi...
method planning (line 63) | def planning(self):
method on_press (line 90) | def on_press(self, event):
method InvalidateNodes (line 130) | def InvalidateNodes(self):
method is_path_invalid (line 135) | def is_path_invalid(self):
method is_collision_obs_add (line 140) | def is_collision_obs_add(self, start, end):
method replanning (line 156) | def replanning(self):
method TrimRRT (line 181) | def TrimRRT(self):
method generate_random_node (line 192) | def generate_random_node(self, goal_sample_rate):
method generate_random_node_replanning (line 201) | def generate_random_node_replanning(self, goal_sample_rate, waypoint_s...
method nearest_neighbor (line 214) | def nearest_neighbor(node_list, n):
method new_state (line 218) | def new_state(self, node_start, node_end):
method extract_path (line 228) | def extract_path(self, node_end):
method extract_waypoint (line 238) | def extract_waypoint(self, node_end):
method get_distance_and_angle (line 249) | def get_distance_and_angle(node_start, node_end):
method plot_grid (line 254) | def plot_grid(self, name):
method plot_visited (line 292) | def plot_visited(self, animation=True):
method plot_vertex_old (line 309) | def plot_vertex_old(self):
method plot_vertex_new (line 314) | def plot_vertex_new(self):
method plot_path (line 328) | def plot_path(path, color='red'):
function main (line 333) | def main():
FILE: Sampling_based_Planning/rrt_2D/env.py
class Env (line 7) | class Env:
method __init__ (line 8) | def __init__(self):
method obs_boundary (line 16) | def obs_boundary():
method obs_rectangle (line 26) | def obs_rectangle():
method obs_circle (line 36) | def obs_circle():
FILE: Sampling_based_Planning/rrt_2D/extended_rrt.py
class Node (line 19) | class Node:
method __init__ (line 20) | def __init__(self, n):
class ExtendedRrt (line 26) | class ExtendedRrt:
method __init__ (line 27) | def __init__(self, s_start, s_goal, step_len, goal_sample_rate, waypoi...
method planning (line 50) | def planning(self):
method on_press (line 76) | def on_press(self, event):
method replanning (line 96) | def replanning(self):
method generate_random_node (line 117) | def generate_random_node(self, goal_sample_rate):
method generate_random_node_replanning (line 126) | def generate_random_node_replanning(self, goal_sample_rate, waypoint_s...
method nearest_neighbor (line 140) | def nearest_neighbor(node_list, n):
method new_state (line 144) | def new_state(self, node_start, node_end):
method extract_path (line 154) | def extract_path(self, node_end):
method extract_waypoint (line 164) | def extract_waypoint(self, node_end):
method get_distance_and_angle (line 175) | def get_distance_and_angle(node_start, node_end):
method plot_grid (line 180) | def plot_grid(self, name):
method plot_visited (line 218) | def plot_visited(self):
method plot_path (line 237) | def plot_path(path, color='red'):
function main (line 242) | def main():
FILE: Sampling_based_Planning/rrt_2D/fast_marching_trees.py
class Node (line 20) | class Node:
method __init__ (line 21) | def __init__(self, n):
class FMT (line 28) | class FMT:
method __init__ (line 29) | def __init__(self, x_start, x_goal, search_radius):
method Init (line 52) | def Init(self):
method Planning (line 62) | def Planning(self):
method ChooseGoalPoint (line 100) | def ChooseGoalPoint(self):
method ExtractPath (line 106) | def ExtractPath(self):
method Cost (line 120) | def Cost(self, x_start, x_end):
method calc_dist (line 127) | def calc_dist(x_start, x_end):
method Near (line 131) | def Near(nodelist, z, rn):
method SampleFree (line 135) | def SampleFree(self):
method animation (line 152) | def animation(self, path_x, path_y, visited):
method plot_grid (line 172) | def plot_grid(self, name):
function main (line 211) | def main():
FILE: Sampling_based_Planning/rrt_2D/informed_rrt_star.py
class Node (line 21) | class Node:
method __init__ (line 22) | def __init__(self, n):
class IRrtStar (line 28) | class IRrtStar:
method __init__ (line 29) | def __init__(self, x_start, x_goal, step_len,
method init (line 54) | def init(self):
method planning (line 63) | def planning(self):
method Steer (line 113) | def Steer(self, x_start, x_goal):
method Near (line 122) | def Near(self, nodelist, node):
method Sample (line 132) | def Sample(self, c_max, c_min, x_center, C):
method SampleUnitBall (line 152) | def SampleUnitBall():
method SampleFreeSpace (line 158) | def SampleFreeSpace(self):
method ExtractPath (line 167) | def ExtractPath(self, node):
method InGoalRegion (line 178) | def InGoalRegion(self, node):
method RotationToWorldFrame (line 185) | def RotationToWorldFrame(x_start, x_goal, L):
method Nearest (line 196) | def Nearest(nodelist, n):
method Line (line 201) | def Line(x_start, x_goal):
method Cost (line 204) | def Cost(self, node):
method get_distance_and_angle (line 219) | def get_distance_and_angle(node_start, node_end):
method animation (line 224) | def animation(self, x_center=None, c_best=None, dist=None, theta=None):
method plot_grid (line 240) | def plot_grid(self, name):
method draw_ellipse (line 279) | def draw_ellipse(x_center, c_best, dist, theta):
function main (line 296) | def main():
FILE: Sampling_based_Planning/rrt_2D/plotting.py
class Plotting (line 17) | class Plotting:
method __init__ (line 18) | def __init__(self, x_start, x_goal):
method animation (line 25) | def animation(self, nodelist, path, name, animation=False):
method animation_connect (line 30) | def animation_connect(self, V1, V2, path, name):
method plot_grid (line 35) | def plot_grid(self, name):
method plot_visited (line 75) | def plot_visited(nodelist, animation):
method plot_visited_connect (line 93) | def plot_visited_connect(V1, V2):
method plot_path (line 113) | def plot_path(path):
FILE: Sampling_based_Planning/rrt_2D/queue.py
class QueueFIFO (line 5) | class QueueFIFO:
method __init__ (line 11) | def __init__(self):
method empty (line 14) | def empty(self):
method put (line 17) | def put(self, node):
method get (line 20) | def get(self):
class QueueLIFO (line 24) | class QueueLIFO:
method __init__ (line 30) | def __init__(self):
method empty (line 33) | def empty(self):
method put (line 36) | def put(self, node):
method get (line 39) | def get(self):
class QueuePrior (line 43) | class QueuePrior:
method __init__ (line 49) | def __init__(self):
method empty (line 52) | def empty(self):
method put (line 55) | def put(self, item, priority):
method get (line 58) | def get(self):
method enumerate (line 61) | def enumerate(self):
FILE: Sampling_based_Planning/rrt_2D/rrt.py
class Node (line 17) | class Node:
method __init__ (line 18) | def __init__(self, n):
class Rrt (line 24) | class Rrt:
method __init__ (line 25) | def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_m...
method planning (line 43) | def planning(self):
method generate_random_node (line 59) | def generate_random_node(self, goal_sample_rate):
method nearest_neighbor (line 69) | def nearest_neighbor(node_list, n):
method new_state (line 73) | def new_state(self, node_start, node_end):
method extract_path (line 83) | def extract_path(self, node_end):
method get_distance_and_angle (line 94) | def get_distance_and_angle(node_start, node_end):
function main (line 100) | def main():
FILE: Sampling_based_Planning/rrt_2D/rrt_connect.py
class Node (line 19) | class Node:
method __init__ (line 20) | def __init__(self, n):
class RrtConnect (line 26) | class RrtConnect:
method __init__ (line 27) | def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_m...
method planning (line 46) | def planning(self):
method change_node (line 82) | def change_node(node_new_prim, node_new_prim2):
method is_node_same (line 89) | def is_node_same(node_new_prim, node_new):
method generate_random_node (line 96) | def generate_random_node(self, sample_goal, goal_sample_rate):
method nearest_neighbor (line 106) | def nearest_neighbor(node_list, n):
method new_state (line 110) | def new_state(self, node_start, node_end):
method extract_path (line 121) | def extract_path(node_new, node_new_prim):
method get_distance_and_angle (line 139) | def get_distance_and_angle(node_start, node_end):
function main (line 145) | def main():
FILE: Sampling_based_Planning/rrt_2D/rrt_star.py
class Node (line 17) | class Node:
method __init__ (line 18) | def __init__(self, n):
class RrtStar (line 24) | class RrtStar:
method __init__ (line 25) | def __init__(self, x_start, x_goal, step_len,
method planning (line 46) | def planning(self):
method new_state (line 68) | def new_state(self, node_start, node_goal):
method choose_parent (line 79) | def choose_parent(self, node_new, neighbor_index):
method rewire (line 85) | def rewire(self, node_new, neighbor_index):
method search_goal_parent (line 92) | def search_goal_parent(self):
method get_new_cost (line 103) | def get_new_cost(self, node_start, node_end):
method generate_random_node (line 108) | def generate_random_node(self, goal_sample_rate):
method find_near_neighbor (line 117) | def find_near_neighbor(self, node_new):
method nearest_neighbor (line 128) | def nearest_neighbor(node_list, n):
method cost (line 133) | def cost(node_p):
method update_cost (line 143) | def update_cost(self, parent_node):
method extract_path (line 157) | def extract_path(self, node_end):
method get_distance_and_angle (line 169) | def get_distance_and_angle(node_start, node_end):
function main (line 175) | def main():
FILE: Sampling_based_Planning/rrt_2D/rrt_star_smart.py
class Node (line 21) | class Node:
method __init__ (line 22) | def __init__(self, n):
class RrtStarSmart (line 28) | class RrtStarSmart:
method __init__ (line 29) | def __init__(self, x_start, x_goal, step_len,
method planning (line 57) | def planning(self):
method PathOptimization (line 107) | def PathOptimization(self, node):
method UpdateBeacons (line 125) | def UpdateBeacons(self):
method ReformObsVertex (line 140) | def ReformObsVertex(self):
method Steer (line 149) | def Steer(self, x_start, x_goal):
method Near (line 158) | def Near(self, nodelist, node):
method Sample (line 168) | def Sample(self, goal=None):
method SampleFreeSpace (line 187) | def SampleFreeSpace(self):
method ExtractPath (line 196) | def ExtractPath(self):
method InitialPathFound (line 208) | def InitialPathFound(self, node):
method Nearest (line 215) | def Nearest(nodelist, n):
method Line (line 220) | def Line(x_start, x_goal):
method Cost (line 224) | def Cost(node):
method get_distance_and_angle (line 236) | def get_distance_and_angle(node_start, node_end):
method animation (line 241) | def animation(self):
method plot_grid (line 263) | def plot_grid(self, name):
function main (line 302) | def main():
FILE: Sampling_based_Planning/rrt_2D/utils.py
class Utils (line 18) | class Utils:
method __init__ (line 19) | def __init__(self):
method update_obs (line 27) | def update_obs(self, obs_cir, obs_bound, obs_rec):
method get_obs_vertex (line 32) | def get_obs_vertex(self):
method is_intersect_rec (line 45) | def is_intersect_rec(self, start, end, o, d, a, b):
method is_intersect_circle (line 67) | def is_intersect_circle(self, o, d, a, r):
method is_collision (line 83) | def is_collision(self, start, end):
method is_inside_obs (line 106) | def is_inside_obs(self, node):
method get_ray (line 126) | def get_ray(start, end):
method get_dist (line 132) | def get_dist(start, end):
FILE: Sampling_based_Planning/rrt_3D/ABIT_star3D.py
class ABIT_star (line 25) | class ABIT_star:
method __init__ (line 27) | def __init__(self):
method run (line 35) | def run(self):
method sample (line 91) | def sample(self, m, xgoal):
method expand (line 94) | def expand(self, set_xi, T, Xunconnected, r):
method prune (line 105) | def prune(self, T, Xunconnected, xgoal):
method g_hat (line 115) | def g_hat(self, x):
method h_hat (line 118) | def h_hat(self, x):
method c_hat (line 121) | def c_hat(self, x1, x2):
method f_hat (line 124) | def f_hat(self, x):
method g_T (line 127) | def g_T(self, x):
method r (line 130) | def r(self, q):
method Lambda (line 133) | def Lambda(self, inputset):
method Zeta (line 136) | def Zeta(self):
method is_search_marked_finished (line 139) | def is_search_marked_finished(self):
method mark_search_unfinished (line 142) | def mark_search_unfinished(self):
method mark_search_finished (line 146) | def mark_search_finished(self):
FILE: Sampling_based_Planning/rrt_3D/BIT_star3D.py
function CreateUnitSphere (line 33) | def CreateUnitSphere(r = 1):
function draw_ellipsoid (line 44) | def draw_ellipsoid(ax, C, L, xcenter):
class BIT_star (line 50) | class BIT_star:
method __init__ (line 52) | def __init__(self, show_ellipse=False):
method run (line 82) | def run(self):
method Sample (line 149) | def Sample(self, m, cmax, bias = 0.05, xrand = set()):
method SampleUnitBall (line 177) | def SampleUnitBall(self, n):
method RotationToWorldFrame (line 188) | def RotationToWorldFrame(self, xstart, xgoal):
method ExpandVertex (line 199) | def ExpandVertex(self, v):
method Prune (line 210) | def Prune(self, c):
method radius (line 217) | def radius(self, q):
method Lambda (line 222) | def Lambda(self, inputset):
method Xf_hat (line 227) | def Xf_hat(self, X):
method Zeta (line 233) | def Zeta(self):
method BestInQueue (line 238) | def BestInQueue(self, inputset, mode):
method BestQueueValue (line 250) | def BestQueueValue(self, inputset, mode):
method g_hat (line 261) | def g_hat(self, v):
method h_hat (line 264) | def h_hat(self, v):
method f_hat (line 267) | def f_hat(self, v):
method c (line 271) | def c(self, v, w):
method c_hat (line 279) | def c_hat(self, v, w):
method g_T (line 284) | def g_T(self, v):
method path (line 291) | def path(self):
method visualization (line 303) | def visualization(self):
FILE: Sampling_based_Planning/rrt_3D/FMT_star3D.py
class FMT_star (line 23) | class FMT_star:
method __init__ (line 25) | def __init__(self, radius = 1, n = 1000):
method generateSampleSet (line 43) | def generateSampleSet(self, n):
method initNodeSets (line 49) | def initNodeSets(self):
method Near (line 68) | def Near(self, nodeset, node, rn):
method Save (line 74) | def Save(self, V_associated, node):
method path (line 77) | def path(self, z, initT):
method Cost (line 89) | def Cost(self, x, y):
method FMTrun (line 96) | def FMTrun(self):
method visualization (line 144) | def visualization(self, ind, E):
FILE: Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py
class dynamic_rrt_3D (line 18) | class dynamic_rrt_3D:
method __init__ (line 20) | def __init__(self):
method RegrowRRT (line 41) | def RegrowRRT(self):
method TrimRRT (line 45) | def TrimRRT(self):
method InvalidateNodes (line 59) | def InvalidateNodes(self, obstacle):
method initRRT (line 66) | def initRRT(self):
method GrowRRT (line 70) | def GrowRRT(self):
method ChooseTarget (line 89) | def ChooseTarget(self):
method RandomState (line 103) | def RandomState(self):
method AddNode (line 108) | def AddNode(self, nearest, extended):
method Nearest (line 114) | def Nearest(self, target):
method Extend (line 118) | def Extend(self, nearest, target):
method Main (line 124) | def Main(self):
method FindAffectedEdges (line 157) | def FindAffectedEdges(self, obstacle):
method ChildEndpointNode (line 169) | def ChildEndpointNode(self, edge):
method CreateTreeFromNodes (line 172) | def CreateTreeFromNodes(self, Nodes):
method PathisInvalid (line 180) | def PathisInvalid(self, path):
method path (line 185) | def path(self, dist=0):
method visualization (line 201) | def visualization(self):
FILE: Sampling_based_Planning/rrt_3D/env3D.py
function R_matrix (line 10) | def R_matrix(z_angle,y_angle,x_angle):
function getblocks (line 22) | def getblocks():
function getballs (line 35) | def getballs():
function getAABB (line 42) | def getAABB(blocks):
function getAABB2 (line 49) | def getAABB2(blocks):
function add_block (line 56) | def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00,...
class aabb (line 59) | class aabb(object):
method __init__ (line 64) | def __init__(self,AABB):
class obb (line 69) | class obb(object):
method __init__ (line 73) | def __init__(self, P, E, O):
class env (line 79) | class env():
method __init__ (line 80) | def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=20, zmax=5, r...
method New_block (line 94) | def New_block(self):
method move_start (line 100) | def move_start(self, x):
method move_block (line 103) | def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], block_to_move ...
method move_OBB (line 131) | def move_OBB(self, obb_to_move = 0, theta=[0,0,0], translation=[0,0,0]):
FILE: Sampling_based_Planning/rrt_3D/extend_rrt3D.py
class extend_rrt (line 23) | class extend_rrt(object):
method __init__ (line 24) | def __init__(self):
method RRTplan (line 41) | def RRTplan(self, env, initial, goal):
method Nearest (line 66) | def Nearest(self, tree, target):
method Extend (line 70) | def Extend(self, env, nearest, target):
method AddNode (line 75) | def AddNode(self, tree, nearest, extended):
method RandomState (line 80) | def RandomState(self):
method ChooseTarget (line 86) | def ChooseTarget(self, state):
FILE: Sampling_based_Planning/rrt_3D/informed_rrt_star3D.py
function CreateUnitSphere (line 25) | def CreateUnitSphere(r = 1):
function draw_ellipsoid (line 36) | def draw_ellipsoid(ax, C, L, xcenter):
class IRRT (line 42) | class IRRT:
method __init__ (line 44) | def __init__(self, show_ellipse = False):
method Informed_rrt (line 65) | def Informed_rrt(self):
method Sample (line 127) | def Sample(self, xstart, xgoal, cmax, bias = 0.05):
method SampleUnitBall (line 151) | def SampleUnitBall(self):
method RotationToWorldFrame (line 162) | def RotationToWorldFrame(self, xstart, xgoal):
method InGoalRegion (line 172) | def InGoalRegion(self, x):
method cost (line 176) | def cost(self, x):
method line (line 185) | def line(self, x, y):
method visualization (line 188) | def visualization(self):
FILE: Sampling_based_Planning/rrt_3D/plot_util3D.py
function CreateSphere (line 10) | def CreateSphere(center, r):
function draw_Spheres (line 20) | def draw_Spheres(ax, balls):
function draw_block_list (line 26) | def draw_block_list(ax, blocks, color=None, alpha=0.15):
function obb_verts (line 47) | def obb_verts(obb):
function draw_obb (line 58) | def draw_obb(ax, OBB, color=None, alpha=0.15):
function draw_line (line 73) | def draw_line(ax, SET, visibility=1, color=None):
function visualization (line 83) | def visualization(initparams):
function set_axes_equal (line 128) | def set_axes_equal(ax):
function make_transparent (line 156) | def make_transparent(ax):
FILE: Sampling_based_Planning/rrt_3D/queue.py
class MinheapPQ (line 7) | class MinheapPQ:
method __init__ (line 12) | def __init__(self):
method put (line 19) | def put(self, item, priority):
method put_set (line 29) | def put_set(self, dictin):
method check_remove (line 34) | def check_remove(self, item):
method check_remove_set (line 41) | def check_remove_set(self, set_input):
method priority_filtering (line 51) | def priority_filtering(self, threshold, mode):
method get (line 69) | def get(self):
method top_key (line 79) | def top_key(self):
method enumerate (line 82) | def enumerate(self):
method allnodes (line 85) | def allnodes(self):
FILE: Sampling_based_Planning/rrt_3D/rrt3D.py
class rrt (line 20) | class rrt():
method __init__ (line 21) | def __init__(self):
method wireup (line 38) | def wireup(self, x, y):
method run (line 42) | def run(self):
FILE: Sampling_based_Planning/rrt_3D/rrt_connect3D.py
class Tree (line 22) | class Tree():
method __init__ (line 23) | def __init__(self, node):
method add_vertex (line 29) | def add_vertex(self, node):
method add_edge (line 33) | def add_edge(self, parent, child):
class rrt_connect (line 38) | class rrt_connect():
method __init__ (line 39) | def __init__(self):
method BUILD_RRT (line 60) | def BUILD_RRT(self, qinit):
method EXTEND (line 67) | def EXTEND(self, tree, q):
method NEAREST_NEIGHBOR (line 80) | def NEAREST_NEIGHBOR(self, q, tree):
method RANDOM_CONFIG (line 89) | def RANDOM_CONFIG(self):
method NEW_CONFIG (line 92) | def NEW_CONFIG(self, q, qnear, qnew, dist = None):
method CONNECT (line 100) | def CONNECT(self, Tree, q):
method RRT_CONNECT_PLANNER (line 108) | def RRT_CONNECT_PLANNER(self, qinit, qgoal):
method SWAP (line 129) | def SWAP(self, tree_a, tree_b):
method PATH (line 133) | def PATH(self, tree_a, tree_b):
method visualization (line 151) | def visualization(self, tree_a, tree_b, index):
FILE: Sampling_based_Planning/rrt_3D/rrt_star3D.py
class rrtstar (line 19) | class rrtstar():
method __init__ (line 20) | def __init__(self):
method wireup (line 40) | def wireup(self,x,y):
method removewire (line 44) | def removewire(self,xnear):
method reached (line 49) | def reached(self):
method run (line 59) | def run(self):
FILE: Sampling_based_Planning/rrt_3D/utils3D.py
function getRay (line 13) | def getRay(x, y):
function getAABB (line 18) | def getAABB(blocks):
function getDist (line 25) | def getDist(pos1, pos2):
function sampleFree (line 41) | def sampleFree(initparams, bias = 0.1):
function isinside (line 55) | def isinside(initparams, x):
function isinbound (line 68) | def isinbound(i, x, mode = False, factor = 0, isarray = False):
function isinobb (line 79) | def isinobb(i, x, isarray = False):
function isinball (line 90) | def isinball(i, x, factor = 0):
function lineSphere (line 95) | def lineSphere(p0, p1, ball):
function lineAABB (line 113) | def lineAABB(p0, p1, dist, aabb):
function lineOBB (line 136) | def lineOBB(p0, p1, dist, obb):
function isCollide (line 147) | def isCollide(initparams, x, child, dist=None):
function nearest (line 170) | def nearest(initparams, x, isset=False):
function near (line 178) | def near(initparams, x):
function steer (line 195) | def steer(initparams, x, y, DIST=False):
function cost (line 209) | def cost(initparams, x):
function cost_from_set (line 215) | def cost_from_set(initparams, x):
function path (line 221) | def path(initparams, Path=[], dist=0):
class edgeset (line 230) | class edgeset(object):
method __init__ (line 231) | def __init__(self):
method add_edge (line 234) | def add_edge(self, edge):
method remove_edge (line 242) | def remove_edge(self, edge):
method get_edge (line 246) | def get_edge(self, nodes = None):
method isEndNode (line 259) | def isEndNode(self, node):
class Node (line 263) | class Node:
method __init__ (line 264) | def __init__(self, data):
function tree_add_edge (line 269) | def tree_add_edge(node_in_tree, x):
function tree_bfs (line 277) | def tree_bfs(head, x):
function tree_nearest (line 289) | def tree_nearest(head, x):
function tree_steer (line 307) | def tree_steer(initparams, node, x):
function tree_print (line 314) | def tree_print(head):
function tree_path (line 331) | def tree_path(initparams, end_node):
class kdTree (line 341) | class kdTree:
method __init__ (line 342) | def __init__(self):
method R1_dist (line 345) | def R1_dist(self, q, p):
method S1_dist (line 348) | def S1_dist(self, q, p):
method P3_dist (line 351) | def P3_dist(self, q, p):
class rrt_demo (line 371) | class rrt_demo:
method __init__ (line 372) | def __init__(self):
method run (line 384) | def run(self):
FILE: Search_based_Planning/Search_2D/ARAstar.py
class AraStar (line 20) | class AraStar:
method __init__ (line 21) | def __init__(self, s_start, s_goal, e, heuristic_type):
method init (line 39) | def init(self):
method searching (line 49) | def searching(self):
method ImprovePath (line 66) | def ImprovePath(self):
method calc_smallest_f (line 100) | def calc_smallest_f(self):
method get_neighbor (line 109) | def get_neighbor(self, s):
method update_e (line 118) | def update_e(self):
method f_value (line 128) | def f_value(self, x):
method extract_path (line 138) | def extract_path(self):
method h (line 156) | def h(self, s):
method cost (line 171) | def cost(self, s_start, s_goal):
method is_collision (line 185) | def is_collision(self, s_start, s_end):
function main (line 210) | def main():
FILE: Search_based_Planning/Search_2D/Anytime_D_star.py
class ADStar (line 18) | class ADStar:
method __init__ (line 19) | def __init__(self, s_start, s_goal, eps, heuristic_type):
method run (line 51) | def run(self):
method on_press (line 75) | def on_press(self, event):
method ComputeOrImprovePath (line 169) | def ComputeOrImprovePath(self):
method UpdateState (line 190) | def UpdateState(self, s):
method Key (line 204) | def Key(self, s):
method TopKey (line 210) | def TopKey(self):
method h (line 218) | def h(self, s_start, s_goal):
method cost (line 226) | def cost(self, s_start, s_goal):
method is_collision (line 240) | def is_collision(self, s_start, s_end):
method get_neighbor (line 257) | def get_neighbor(self, s):
method extract_path (line 266) | def extract_path(self):
method plot_path (line 287) | def plot_path(self, path):
method plot_visited (line 294) | def plot_visited(self):
function main (line 308) | def main():
FILE: Search_based_Planning/Search_2D/Astar.py
class AStar (line 17) | class AStar:
method __init__ (line 20) | def __init__(self, s_start, s_goal, heuristic_type):
method searching (line 35) | def searching(self):
method searching_repeated_astar (line 67) | def searching_repeated_astar(self, e):
method repeated_searching (line 84) | def repeated_searching(self, s_start, s_goal, e):
method get_neighbor (line 120) | def get_neighbor(self, s):
method cost (line 129) | def cost(self, s_start, s_goal):
method is_collision (line 143) | def is_collision(self, s_start, s_end):
method f_value (line 167) | def f_value(self, s):
method extract_path (line 176) | def extract_path(self, PARENT):
method heuristic (line 194) | def heuristic(self, s):
function main (line 210) | def main():
FILE: Search_based_Planning/Search_2D/Best_First.py
class BestFirst (line 18) | class BestFirst(AStar):
method searching (line 21) | def searching(self):
function main (line 56) | def main():
FILE: Search_based_Planning/Search_2D/Bidirectional_a_star.py
class BidirectionalAStar (line 17) | class BidirectionalAStar:
method __init__ (line 18) | def __init__(self, s_start, s_goal, heuristic_type):
method init (line 37) | def init(self):
method searching (line 53) | def searching(self):
method get_neighbor (line 107) | def get_neighbor(self, s):
method extract_path (line 116) | def extract_path(self, s_meet):
method f_value_fore (line 145) | def f_value_fore(self, s):
method f_value_back (line 154) | def f_value_back(self, s):
method h (line 163) | def h(self, s, goal):
method cost (line 178) | def cost(self, s_start, s_goal):
method is_collision (line 192) | def is_collision(self, s_start, s_end):
function main (line 217) | def main():
FILE: Search_based_Planning/Search_2D/D_star.py
class DStar (line 17) | class DStar:
method __init__ (line 18) | def __init__(self, s_start, s_goal):
method init (line 40) | def init(self):
method run (line 50) | def run(self, s_start, s_end):
method on_press (line 65) | def on_press(self, event):
method extract_path (line 95) | def extract_path(self, s_start, s_end):
method process_state (line 104) | def process_state(self):
method min_state (line 164) | def min_state(self):
method get_k_min (line 175) | def get_k_min(self):
method insert (line 186) | def insert(self, s, h_new):
method delete (line 204) | def delete(self, s):
method modify (line 215) | def modify(self, s):
method modify_cost (line 229) | def modify_cost(self, s):
method get_neighbor (line 236) | def get_neighbor(self, s):
method cost (line 246) | def cost(self, s_start, s_goal):
method is_collision (line 260) | def is_collision(self, s_start, s_end):
method plot_path (line 277) | def plot_path(self, path):
method plot_visited (line 284) | def plot_visited(self, visited):
function main (line 296) | def main():
FILE: Search_based_Planning/Search_2D/D_star_Lite.py
class DStar (line 17) | class DStar:
method __init__ (line 18) | def __init__(self, s_start, s_goal, heuristic_type):
method run (line 44) | def run(self):
method on_press (line 51) | def on_press(self, event):
method ComputePath (line 96) | def ComputePath(self):
method UpdateVertex (line 119) | def UpdateVertex(self, s):
method CalculateKey (line 130) | def CalculateKey(self, s):
method TopKey (line 134) | def TopKey(self):
method h (line 142) | def h(self, s_start, s_goal):
method cost (line 150) | def cost(self, s_start, s_goal):
method is_collision (line 164) | def is_collision(self, s_start, s_end):
method get_neighbor (line 181) | def get_neighbor(self, s):
method extract_path (line 190) | def extract_path(self):
method plot_path (line 211) | def plot_path(self, path):
method plot_visited (line 218) | def plot_visited(self, visited):
function main (line 230) | def main():
FILE: Search_based_Planning/Search_2D/Dijkstra.py
class Dijkstra (line 19) | class Dijkstra(AStar):
method searching (line 22) | def searching(self):
function main (line 57) | def main():
FILE: Search_based_Planning/Search_2D/LPAstar.py
class LPAStar (line 17) | class LPAStar:
method __init__ (line 18) | def __init__(self, s_start, s_goal, heuristic_type):
method run (line 44) | def run(self):
method on_press (line 53) | def on_press(self, event):
method ComputeShortestPath (line 83) | def ComputeShortestPath(self):
method UpdateVertex (line 109) | def UpdateVertex(self, s):
method TopKey (line 131) | def TopKey(self):
method CalculateKey (line 140) | def CalculateKey(self, s):
method get_neighbor (line 145) | def get_neighbor(self, s):
method h (line 161) | def h(self, s):
method cost (line 176) | def cost(self, s_start, s_goal):
method is_collision (line 190) | def is_collision(self, s_start, s_end):
method extract_path (line 207) | def extract_path(self):
method plot_path (line 228) | def plot_path(self, path):
method plot_visited (line 235) | def plot_visited(self, visited):
function main (line 247) | def main():
FILE: Search_based_Planning/Search_2D/LRTAstar.py
class LrtAStarN (line 17) | class LrtAStarN:
method __init__ (line 18) | def __init__(self, s_start, s_goal, N, heuristic_type):
method init (line 32) | def init(self):
method searching (line 42) | def searching(self):
method extract_path_in_CLOSE (line 61) | def extract_path_in_CLOSE(self, s_start, h_value):
method iteration (line 81) | def iteration(self, CLOSED):
method AStar (line 101) | def AStar(self, x_start, N):
method get_neighbor (line 135) | def get_neighbor(self, s):
method extract_path (line 151) | def extract_path(self, x_start, parent):
method h (line 170) | def h(self, s):
method cost (line 185) | def cost(self, s_start, s_goal):
method is_collision (line 199) | def is_collision(self, s_start, s_end):
function main (line 217) | def main():
FILE: Search_based_Planning/Search_2D/RTAAStar.py
class RTAAStar (line 17) | class RTAAStar:
method __init__ (line 18) | def __init__(self, s_start, s_goal, N, heuristic_type):
method init (line 32) | def init(self):
method searching (line 42) | def searching(self):
method cal_h_value (line 62) | def cal_h_value(self, OPEN, CLOSED, g_table, PARENT):
method iteration (line 74) | def iteration(self, CLOSED):
method Astar (line 94) | def Astar(self, x_start, N):
method get_neighbor (line 128) | def get_neighbor(self, s):
method extract_path_in_CLOSE (line 144) | def extract_path_in_CLOSE(self, s_end, s_start, h_value):
method extract_path (line 160) | def extract_path(self, x_start, parent):
method h (line 177) | def h(self, s):
method cost (line 192) | def cost(self, s_start, s_goal):
method is_collision (line 206) | def is_collision(self, s_start, s_end):
function main (line 224) | def main():
FILE: Search_based_Planning/Search_2D/bfs.py
class BFS (line 18) | class BFS(AStar):
method searching (line 21) | def searching(self):
function main (line 57) | def main():
FILE: Search_based_Planning/Search_2D/dfs.py
class DFS (line 13) | class DFS(AStar):
method searching (line 16) | def searching(self):
function main (line 52) | def main():
FILE: Search_based_Planning/Search_2D/env.py
class Env (line 7) | class Env:
method __init__ (line 8) | def __init__(self):
method update_obs (line 15) | def update_obs(self, obs):
method obs_map (line 18) | def obs_map(self):
FILE: Search_based_Planning/Search_2D/plotting.py
class Plotting (line 16) | class Plotting:
method __init__ (line 17) | def __init__(self, xI, xG):
method update_obs (line 22) | def update_obs(self, obs):
method animation (line 25) | def animation(self, path, visited, name):
method animation_lrta (line 31) | def animation_lrta(self, path, visited, name):
method animation_ara_star (line 47) | def animation_ara_star(self, path, visited, name):
method animation_bi_astar (line 58) | def animation_bi_astar(self, path, v_fore, v_back, name):
method plot_grid (line 64) | def plot_grid(self, name):
method plot_visited (line 74) | def plot_visited(self, visited, cl='gray'):
method plot_path (line 102) | def plot_path(self, path, cl='r', flag=False):
method plot_visited_bi (line 116) | def plot_visited_bi(self, v_fore, v_back):
method color_list (line 139) | def color_list():
method color_list_2 (line 153) | def color_list_2():
FILE: Search_based_Planning/Search_2D/queue.py
class QueueFIFO (line 5) | class QueueFIFO:
method __init__ (line 11) | def __init__(self):
method empty (line 14) | def empty(self):
method put (line 17) | def put(self, node):
method get (line 20) | def get(self):
class QueueLIFO (line 24) | class QueueLIFO:
method __init__ (line 30) | def __init__(self):
method empty (line 33) | def empty(self):
method put (line 36) | def put(self, node):
method get (line 39) | def get(self):
class QueuePrior (line 43) | class QueuePrior:
method __init__ (line 49) | def __init__(self):
method empty (line 52) | def empty(self):
method put (line 55) | def put(self, item, priority):
method get (line 58) | def get(self):
method enumerate (line 61) | def enumerate(self):
FILE: Search_based_Planning/Search_3D/Anytime_Dstar3D.py
class Anytime_Dstar (line 19) | class Anytime_Dstar(object):
method __init__ (line 21) | def __init__(self, resolution=1):
method getcost (line 58) | def getcost(self, xi, xj):
method getchildren (line 68) | def getchildren(self, xi):
method geth (line 74) | def geth(self, xi):
method getg (line 80) | def getg(self, xi):
method getrhs (line 85) | def getrhs(self, xi):
method updatecost (line 90) | def updatecost(self, range_changed=None, new=None, old=None, mode=False):
method isinobs (line 105) | def isinobs(self, obs, x, mode):
method key (line 129) | def key(self, s, epsilon=1):
method UpdateState (line 135) | def UpdateState(self, s):
method ComputeorImprovePath (line 148) | def ComputeorImprovePath(self):
method Main (line 168) | def Main(self):
method path (line 219) | def path(self, s_start=None):
FILE: Search_based_Planning/Search_3D/Astar3D.py
class Weighted_A_star (line 21) | class Weighted_A_star(object):
method __init__ (line 22) | def __init__(self, resolution=0.5):
method run (line 47) | def run(self, N=None):
method path (line 89) | def path(self):
method reset (line 100) | def reset(self, xj):
FILE: Search_based_Planning/Search_3D/Dstar3D.py
class D_star (line 16) | class D_star(object):
method __init__ (line 17) | def __init__(self, resolution=1):
method checkState (line 43) | def checkState(self, y):
method get_kmin (line 49) | def get_kmin(self):
method min_state (line 56) | def min_state(self):
method insert (line 67) | def insert(self, x, h_new):
method process_state (line 79) | def process_state(self):
method modify_cost (line 124) | def modify_cost(self, x):
method modify (line 129) | def modify(self, x):
method path (line 137) | def path(self, goal=None):
method run (line 149) | def run(self):
FILE: Search_based_Planning/Search_3D/DstarLite3D.py
class D_star_Lite (line 16) | class D_star_Lite(object):
method __init__ (line 18) | def __init__(self, resolution = 1):
method updatecost (line 53) | def updatecost(self, range_changed=None, new=None, old=None, mode=False):
method getcost (line 65) | def getcost(self, xi, xj):
method getchildren (line 75) | def getchildren(self, xi):
method geth (line 81) | def geth(self, xi):
method getg (line 87) | def getg(self, xi):
method getrhs (line 92) | def getrhs(self, xi):
method CalculateKey (line 98) | def CalculateKey(self, s, epsilion = 1):
method UpdateVertex (line 101) | def UpdateVertex(self, u):
method ComputeShortestPath (line 114) | def ComputeShortestPath(self):
method main (line 136) | def main(self):
method path (line 187) | def path(self, s_start=None):
FILE: Search_based_Planning/Search_3D/LP_Astar3D.py
class Lifelong_Astar (line 18) | class Lifelong_Astar(object):
method __init__ (line 19) | def __init__(self,resolution = 1):
method costset (line 55) | def costset(self):
method getCOSTset (line 71) | def getCOSTset(self,xi,xj):
method children (line 79) | def children(self, x):
method getCHILDRENset (line 88) | def getCHILDRENset(self):
method isCollide (line 92) | def isCollide(self, x, child):
method cost (line 112) | def cost(self, x, y):
method key (line 117) | def key(self,xi,epsilion = 1):
method path (line 120) | def path(self):
method UpdateMembership (line 142) | def UpdateMembership(self, xi, xparent=None):
method ComputePath (line 149) | def ComputePath(self):
method change_env (line 173) | def change_env(self):
FILE: Search_based_Planning/Search_3D/LRT_Astar3D.py
class LRT_A_star2 (line 21) | class LRT_A_star2:
method __init__ (line 22) | def __init__(self, resolution=0.5, N=7):
method updateHeuristic (line 27) | def updateHeuristic(self):
method move (line 45) | def move(self):
method run (line 70) | def run(self):
FILE: Search_based_Planning/Search_3D/RTA_Astar3D.py
class RTA_A_star (line 21) | class RTA_A_star:
method __init__ (line 22) | def __init__(self, resolution=0.5, N=7):
method updateHeuristic (line 29) | def updateHeuristic(self):
method move (line 43) | def move(self):
method run (line 61) | def run(self):
FILE: Search_based_Planning/Search_3D/bidirectional_Astar3D.py
class Weighted_A_star (line 21) | class Weighted_A_star(object):
method __init__ (line 22) | def __init__(self,resolution=0.5):
method run (line 44) | def run(self):
method evaluation (line 67) | def evaluation(self, allchild, xi, conf):
method path (line 94) | def path(self):
FILE: Search_based_Planning/Search_3D/env3D.py
function R_matrix (line 10) | def R_matrix(z_angle,y_angle,x_angle):
function getblocks (line 22) | def getblocks():
function getballs (line 35) | def getballs():
function getAABB (line 42) | def getAABB(blocks):
function getAABB2 (line 49) | def getAABB2(blocks):
function add_block (line 56) | def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00,...
class aabb (line 59) | class aabb(object):
method __init__ (line 64) | def __init__(self,AABB):
class obb (line 69) | class obb(object):
method __init__ (line 73) | def __init__(self, P, E, O):
class env (line 79) | class env():
method __init__ (line 80) | def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=20, zmax=5, r...
method New_block (line 94) | def New_block(self):
method move_start (line 100) | def move_start(self, x):
method move_block (line 103) | def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], block_to_move ...
method move_OBB (line 131) | def move_OBB(self, obb_to_move = 0, theta=[0,0,0], translation=[0,0,0]):
FILE: Search_based_Planning/Search_3D/plot_util3D.py
function CreateSphere (line 9) | def CreateSphere(center,r):
function draw_Spheres (line 18) | def draw_Spheres(ax,balls):
function draw_block_list (line 23) | def draw_block_list(ax, blocks ,color=None,alpha=0.15):
function obb_verts (line 45) | def obb_verts(obb):
function draw_obb (line 56) | def draw_obb(ax, OBB, color=None,alpha=0.15):
function draw_line (line 73) | def draw_line(ax,SET,visibility=1,color=None):
function visualization (line 82) | def visualization(initparams):
function set_axes_equal (line 116) | def set_axes_equal(ax):
function make_transparent (line 144) | def make_transparent(ax):
FILE: Search_based_Planning/Search_3D/queue.py
class QueueFIFO (line 5) | class QueueFIFO:
method __init__ (line 11) | def __init__(self):
method empty (line 14) | def empty(self):
method put (line 17) | def put(self, node):
method get (line 20) | def get(self):
class QueueLIFO (line 24) | class QueueLIFO:
method __init__ (line 30) | def __init__(self):
method empty (line 33) | def empty(self):
method put (line 36) | def put(self, node):
method get (line 39) | def get(self):
class QueuePrior (line 43) | class QueuePrior:
method __init__ (line 49) | def __init__(self):
method empty (line 52) | def empty(self):
method put (line 55) | def put(self, item, priority):
method get (line 58) | def get(self):
method enumerate (line 61) | def enumerate(self):
method check_remove (line 64) | def check_remove(self, item):
method top_key (line 69) | def top_key(self):
class MinheapPQ (line 72) | class MinheapPQ:
method __init__ (line 77) | def __init__(self):
method put (line 84) | def put(self, item, priority):
method check_remove (line 94) | def check_remove(self, item):
method get (line 101) | def get(self):
method top_key (line 111) | def top_key(self):
method enumerate (line 114) | def enumerate(self):
method allnodes (line 117) | def allnodes(self):
FILE: Search_based_Planning/Search_3D/utils3D.py
function getRay (line 7) | def getRay(x, y):
function getDist (line 12) | def getDist(pos1, pos2):
function getManDist (line 16) | def getManDist(pos1, pos2):
function getNearest (line 20) | def getNearest(Space, pt):
function Heuristic (line 30) | def Heuristic(Space, t):
function heuristic_fun (line 37) | def heuristic_fun(initparams, k, t=None):
function isinbound (line 42) | def isinbound(i, x, mode = False, factor = 0, isarray = False):
function isinball (line 53) | def isinball(i, x, factor = 0):
function isinobb (line 58) | def isinobb(i, x, isarray = False):
function OBB2AABB (line 69) | def OBB2AABB(obb):
function lineSphere (line 85) | def lineSphere(p0, p1, ball):
function lineAABB (line 103) | def lineAABB(p0, p1, dist, aabb):
function lineOBB (line 126) | def lineOBB(p0, p1, dist, obb):
function OBBOBB (line 137) | def OBBOBB(obb1, obb2):
function StateSpace (line 240) | def StateSpace(env, factor=0):
function g_Space (line 256) | def g_Space(initparams):
function isCollide (line 265) | def isCollide(initparams, x, child, dist=None):
function children (line 287) | def children(initparams, x, settings = 0):
function obstacleFree (line 308) | def obstacleFree(initparams, x):
function cost (line 318) | def cost(initparams, i, j, dist=None, settings='Euclidean'):
function initcost (line 338) | def initcost(initparams):
Condensed preview — 75 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (420K chars).
[
{
"path": ".idea/.gitignore",
"chars": 39,
"preview": "\n# Default ignored files\n/workspace.xml"
},
{
"path": ".idea/PathPlanning.iml",
"chars": 486,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<module type=\"PYTHON_MODULE\" version=\"4\">\n <component name=\"NewModuleRootManager"
},
{
"path": ".idea/inspectionProfiles/profiles_settings.xml",
"chars": 174,
"preview": "<component name=\"InspectionProjectProfileManager\">\n <settings>\n <option name=\"USE_PROJECT_PROFILE\" value=\"false\" />\n"
},
{
"path": ".idea/misc.xml",
"chars": 185,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<project version=\"4\">\n <component name=\"ProjectRootManager\" version=\"2\" project-"
},
{
"path": ".idea/modules.xml",
"chars": 276,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<project version=\"4\">\n <component name=\"ProjectModuleManager\">\n <modules>\n "
},
{
"path": ".idea/vcs.xml",
"chars": 180,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<project version=\"4\">\n <component name=\"VcsDirectoryMappings\">\n <mapping dire"
},
{
"path": "CurvesGenerator/bezier_path.py",
"chars": 4196,
"preview": "\"\"\"\nbezier path\n\nauthor: Atsushi Sakai(@Atsushi_twi)\nmodified: huiming zhou\n\"\"\"\n\nimport numpy as np\nimport matplotlib.py"
},
{
"path": "CurvesGenerator/bspline_curve.py",
"chars": 2333,
"preview": "\"\"\"\n\nPath Planner with B-Spline\n\nauthor: Atsushi Sakai (@Atsushi_twi)\n\n\"\"\"\n\nimport numpy as np\nimport matplotlib.pyplot "
},
{
"path": "CurvesGenerator/cubic_spline.py",
"chars": 6020,
"preview": "#! /usr/bin/python\n# -*- coding: utf-8 -*-\nu\"\"\"\nCubic Spline library on python\n\nauthor Atsushi Sakai\n\nusage: see test co"
},
{
"path": "CurvesGenerator/draw.py",
"chars": 1991,
"preview": "import matplotlib.pyplot as plt\nimport numpy as np\nPI = np.pi\n\n\nclass Arrow:\n def __init__(self, x, y, theta, L, c):\n"
},
{
"path": "CurvesGenerator/dubins_path.py",
"chars": 9343,
"preview": "\"\"\"\nDubins Path\n\"\"\"\n\nimport math\nimport numpy as np\nimport matplotlib.pyplot as plt\nfrom scipy.spatial.transform import "
},
{
"path": "CurvesGenerator/quartic_polynomial.py",
"chars": 945,
"preview": "\"\"\"\nQuartic Polynomial\n\"\"\"\n\nimport numpy as np\n\n\nclass QuarticPolynomial:\n def __init__(self, x0, v0, a0, v1, a1, T):"
},
{
"path": "CurvesGenerator/quintic_polynomial.py",
"chars": 3884,
"preview": "\"\"\"\nQuintic Polynomial\n\"\"\"\n\nimport math\nimport numpy as np\nimport matplotlib.pyplot as plt\n\nimport draw\n\n\nclass QuinticP"
},
{
"path": "CurvesGenerator/reeds_shepp.py",
"chars": 18559,
"preview": "import math\nimport numpy as np\nimport matplotlib.pyplot as plt\n\nimport draw\n\n# parameters initiation\nSTEP_SIZE = 0.2\nMAX"
},
{
"path": "LICENSE",
"chars": 1069,
"preview": "MIT License\n\nCopyright (c) 2020 Huiming Zhou\n\nPermission is hereby granted, free of charge, to any person obtaining a co"
},
{
"path": "README.md",
"chars": 9580,
"preview": "Overview\n------\nThis repository implements some common path planning algorithms used in robotics, including Search-based"
},
{
"path": "Sampling_based_Planning/rrt_2D/adaptively_informed_trees.py",
"chars": 0,
"preview": ""
},
{
"path": "Sampling_based_Planning/rrt_2D/advanced_batch_informed_trees.py",
"chars": 0,
"preview": ""
},
{
"path": "Sampling_based_Planning/rrt_2D/batch_informed_trees.py",
"chars": 13165,
"preview": "\"\"\"\nBatch Informed Trees (BIT*)\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport random\nimport numpy a"
},
{
"path": "Sampling_based_Planning/rrt_2D/dubins_rrt_star.py",
"chars": 10128,
"preview": "\"\"\"\nDUBINS_RRT_STAR 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport random\nimport numpy as np\nimpo"
},
{
"path": "Sampling_based_Planning/rrt_2D/dynamic_rrt.py",
"chars": 11553,
"preview": "\"\"\"\nDYNAMIC_RRT_2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport copy\nimport numpy as np\nimport mat"
},
{
"path": "Sampling_based_Planning/rrt_2D/env.py",
"chars": 946,
"preview": "\"\"\"\nEnvironment for rrt_2D\n@author: huiming zhou\n\"\"\"\n\n\nclass Env:\n def __init__(self):\n self.x_range = (0, 50)"
},
{
"path": "Sampling_based_Planning/rrt_2D/extended_rrt.py",
"chars": 8335,
"preview": "\"\"\"\nEXTENDED_RRT_2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport numpy as np\nimport matplotlib.pyp"
},
{
"path": "Sampling_based_Planning/rrt_2D/fast_marching_trees.py",
"chars": 6249,
"preview": "\"\"\"\nFast Marching Trees (FMT*)\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport random\nimport numpy as"
},
{
"path": "Sampling_based_Planning/rrt_2D/informed_rrt_star.py",
"chars": 9996,
"preview": "\"\"\"\nINFORMED_RRT_STAR 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport random\nimport numpy as np\nim"
},
{
"path": "Sampling_based_Planning/rrt_2D/plotting.py",
"chars": 3651,
"preview": "\"\"\"\nPlotting tools for Sampling-based algorithms\n@author: huiming zhou\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport matpl"
},
{
"path": "Sampling_based_Planning/rrt_2D/queue.py",
"chars": 1324,
"preview": "import collections\nimport heapq\n\n\nclass QueueFIFO:\n \"\"\"\n Class: QueueFIFO\n Description: QueueFIFO is designed f"
},
{
"path": "Sampling_based_Planning/rrt_2D/rrt.py",
"chars": 3450,
"preview": "\"\"\"\nRRT_2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport numpy as np\n\nsys.path.append(os.path.dirna"
},
{
"path": "Sampling_based_Planning/rrt_2D/rrt_connect.py",
"chars": 5051,
"preview": "\"\"\"\nRRT_CONNECT_2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport copy\nimport numpy as np\nimport mat"
},
{
"path": "Sampling_based_Planning/rrt_2D/rrt_sharp.py",
"chars": 0,
"preview": ""
},
{
"path": "Sampling_based_Planning/rrt_2D/rrt_star.py",
"chars": 5912,
"preview": "\"\"\"\nRRT_star 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport numpy as np\n\nsys.path.append(os.path."
},
{
"path": "Sampling_based_Planning/rrt_2D/rrt_star_smart.py",
"chars": 9375,
"preview": "\"\"\"\nRRT_STAR_SMART 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport random\nimport numpy as np\nimpor"
},
{
"path": "Sampling_based_Planning/rrt_2D/utils.py",
"chars": 3840,
"preview": "\"\"\"\nutils for collision check\n@author: huiming zhou\n\"\"\"\n\nimport math\nimport numpy as np\nimport os\nimport sys\n\nsys.path.a"
},
{
"path": "Sampling_based_Planning/rrt_3D/ABIT_star3D.py",
"chars": 5513,
"preview": "# This is Advanced Batched Informed Tree star 3D algorithm \n# implementation\n\"\"\"\nThis is ABIT* code for 3D\n@author: yue "
},
{
"path": "Sampling_based_Planning/rrt_3D/BIT_star3D.py",
"chars": 14462,
"preview": "# This is Batched Informed Tree star 3D algorithm \n# implementation\n\"\"\"\nThis is ABIT* code for 3D\n@author: yue qi \nAlgor"
},
{
"path": "Sampling_based_Planning/rrt_3D/FMT_star3D.py",
"chars": 7061,
"preview": "\"\"\"\nThis is fast marching tree* code for 3D\n@author: yue qi \nsource: Janson, Lucas, et al. \"Fast marching tree: A fast m"
},
{
"path": "Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py",
"chars": 8021,
"preview": "\"\"\"\nThis is dynamic rrt code for 3D\n@author: yue qi\n\"\"\"\nimport numpy as np\nimport time\nimport matplotlib.pyplot as plt\n\n"
},
{
"path": "Sampling_based_Planning/rrt_3D/env3D.py",
"chars": 6377,
"preview": "# this is the three dimensional configuration space for rrt\r\n# !/usr/bin/env python3\r\n# -*- coding: utf-8 -*-\r\n\"\"\"\r\n@aut"
},
{
"path": "Sampling_based_Planning/rrt_3D/extend_rrt3D.py",
"chars": 3362,
"preview": "# Real-Time Randomized Path Planning for Robot Navigation∗\n\"\"\"\nThis is rrt extend code for 3D\n@author: yue qi\n\"\"\"\nimport"
},
{
"path": "Sampling_based_Planning/rrt_3D/informed_rrt_star3D.py",
"chars": 8946,
"preview": "# informed RRT star in 3D\n\"\"\"\nThis is IRRT* code for 3D\n@author: yue qi \nsource: J. D. Gammell, S. S. Srinivasa, and T. "
},
{
"path": "Sampling_based_Planning/rrt_3D/plot_util3D.py",
"chars": 6396,
"preview": "# plotting\r\nimport matplotlib.pyplot as plt\r\nfrom mpl_toolkits.mplot3d import Axes3D\r\nfrom mpl_toolkits.mplot3d.art3d im"
},
{
"path": "Sampling_based_Planning/rrt_3D/queue.py",
"chars": 2994,
"preview": "# min heap used in the FMT*\n\nimport collections\nimport heapq\nimport itertools\n\nclass MinheapPQ:\n \"\"\"\n A priority q"
},
{
"path": "Sampling_based_Planning/rrt_3D/rrt3D.py",
"chars": 1973,
"preview": "\"\"\"\nThis is rrt star code for 3D\n@author: yue qi\n\"\"\"\nimport numpy as np\nfrom numpy.matlib import repmat\nfrom collections"
},
{
"path": "Sampling_based_Planning/rrt_3D/rrt_connect3D.py",
"chars": 6133,
"preview": "# rrt connect algorithm\n\"\"\"\nThis is rrt connect implementation for 3D\n@author: yue qi\n\"\"\"\nimport numpy as np\nfrom numpy."
},
{
"path": "Sampling_based_Planning/rrt_3D/rrt_star3D.py",
"chars": 3399,
"preview": "\"\"\"\nThis is rrt star code for 3D\n@author: yue qi\n\"\"\"\nimport numpy as np\nfrom numpy.matlib import repmat\nfrom collections"
},
{
"path": "Sampling_based_Planning/rrt_3D/rrt_star_smart3D.py",
"chars": 0,
"preview": ""
},
{
"path": "Sampling_based_Planning/rrt_3D/utils3D.py",
"chars": 13736,
"preview": "import numpy as np\r\nfrom numpy.matlib import repmat\r\nimport pyrr as pyrr\r\nfrom collections import deque\r\n\r\nimport os\r\nim"
},
{
"path": "Search_based_Planning/Search_2D/ARAstar.py",
"chars": 6897,
"preview": "\"\"\"\nARA_star 2D (Anytime Repairing A*)\n@author: huiming zhou\n\n@description: local inconsistency: g-value decreased.\ng(s)"
},
{
"path": "Search_based_Planning/Search_2D/Anytime_D_star.py",
"chars": 10161,
"preview": "\"\"\"\nAnytime_D_star 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport matplotlib.pyplot as plt\n\nsys.p"
},
{
"path": "Search_based_Planning/Search_2D/Astar.py",
"chars": 6277,
"preview": "\"\"\"\nA_star 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport heapq\n\nsys.path.append(os.path.dirname("
},
{
"path": "Search_based_Planning/Search_2D/Best_First.py",
"chars": 1720,
"preview": "\"\"\"\nBest-First Searching\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport heapq\n\nsys.path.append(os.pa"
},
{
"path": "Search_based_Planning/Search_2D/Bidirectional_a_star.py",
"chars": 6914,
"preview": "\"\"\"\nBidirectional_a_star 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport heapq\n\nsys.path.append(os"
},
{
"path": "Search_based_Planning/Search_2D/D_star.py",
"chars": 9423,
"preview": "\"\"\"\nD_star 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport matplotlib.pyplot as plt\n\nsys.path.appe"
},
{
"path": "Search_based_Planning/Search_2D/D_star_Lite.py",
"chars": 7422,
"preview": "\"\"\"\nD_star_Lite 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport matplotlib.pyplot as plt\n\nsys.path"
},
{
"path": "Search_based_Planning/Search_2D/Dijkstra.py",
"chars": 1671,
"preview": "\"\"\"\nDijkstra 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport heapq\n\nsys.path.append(os.path.dirnam"
},
{
"path": "Search_based_Planning/Search_2D/LPAstar.py",
"chars": 7353,
"preview": "\"\"\"\nLPA_star 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport matplotlib.pyplot as plt\n\nsys.path.ap"
},
{
"path": "Search_based_Planning/Search_2D/LRTAstar.py",
"chars": 7055,
"preview": "\"\"\"\nLRTA_star 2D (Learning Real-time A*)\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport copy\nimport math\n\nsys.pa"
},
{
"path": "Search_based_Planning/Search_2D/RTAAStar.py",
"chars": 7316,
"preview": "\"\"\"\nRTAAstar 2D (Real-time Adaptive A*)\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport copy\nimport math\n\nsys.pat"
},
{
"path": "Search_based_Planning/Search_2D/bfs.py",
"chars": 1781,
"preview": "\"\"\"\nBreadth-first Searching_2D (BFS)\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nfrom collections import deque\n\nsys."
},
{
"path": "Search_based_Planning/Search_2D/dfs.py",
"chars": 1745,
"preview": "\nimport os\nimport sys\nimport math\nimport heapq\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n "
},
{
"path": "Search_based_Planning/Search_2D/env.py",
"chars": 1036,
"preview": "\"\"\"\nEnv 2D\n@author: huiming zhou\n\"\"\"\n\n\nclass Env:\n def __init__(self):\n self.x_range = 51 # size of backgroun"
},
{
"path": "Search_based_Planning/Search_2D/plotting.py",
"chars": 4592,
"preview": "\"\"\"\nPlot tools 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport matplotlib.pyplot as plt\n\nsys.path.append(os.pa"
},
{
"path": "Search_based_Planning/Search_2D/queue.py",
"chars": 1324,
"preview": "import collections\nimport heapq\n\n\nclass QueueFIFO:\n \"\"\"\n Class: QueueFIFO\n Description: QueueFIFO is designed f"
},
{
"path": "Search_based_Planning/Search_3D/Anytime_Dstar3D.py",
"chars": 9303,
"preview": "# check paper of \n# [Likhachev2005]\nimport numpy as np\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\nfrom collec"
},
{
"path": "Search_based_Planning/Search_3D/Astar3D.py",
"chars": 4389,
"preview": "# this is the three dimensional A* algo\n# !/usr/bin/env python3\n# -*- coding: utf-8 -*-\n\"\"\"\n@author: yue qi\n\"\"\"\nimport n"
},
{
"path": "Search_based_Planning/Search_3D/Dstar3D.py",
"chars": 7146,
"preview": "import numpy as np\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\nfrom collections import defaultdict\n\nsys.path.a"
},
{
"path": "Search_based_Planning/Search_3D/DstarLite3D.py",
"chars": 9200,
"preview": "import numpy as np\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\nfrom collections import defaultdict\n\nsys.path.a"
},
{
"path": "Search_based_Planning/Search_3D/LP_Astar3D.py",
"chars": 6753,
"preview": "import numpy as np\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspat"
},
{
"path": "Search_based_Planning/Search_3D/LRT_Astar3D.py",
"chars": 2850,
"preview": "# this is the three dimensional N>1 LRTA* algo\n# !/usr/bin/env python3\n# -*- coding: utf-8 -*-\n\"\"\"\n@author: yue qi\n\"\"\"\ni"
},
{
"path": "Search_based_Planning/Search_3D/RTA_Astar3D.py",
"chars": 2463,
"preview": "# this is the three dimensional Real-time Adaptive LRTA* algo\n# !/usr/bin/env python3\n# -*- coding: utf-8 -*-\n\"\"\"\n@autho"
},
{
"path": "Search_based_Planning/Search_3D/bidirectional_Astar3D.py",
"chars": 4568,
"preview": "# this is the three dimensional bidirectional A* algo\n# !/usr/bin/env python3\n# -*- coding: utf-8 -*-\n\"\"\"\n@author: yue q"
},
{
"path": "Search_based_Planning/Search_3D/env3D.py",
"chars": 6178,
"preview": "# this is the three dimensional space\n# !/usr/bin/env python3\n# -*- coding: utf-8 -*-\n\"\"\"\n@author: yue qi\n\"\"\"\nimport num"
},
{
"path": "Search_based_Planning/Search_3D/plot_util3D.py",
"chars": 5878,
"preview": "# plotting\nimport matplotlib.pyplot as plt\nfrom mpl_toolkits.mplot3d import Axes3D\nfrom mpl_toolkits.mplot3d.art3d impor"
},
{
"path": "Search_based_Planning/Search_3D/queue.py",
"chars": 4101,
"preview": "import collections\nimport heapq\nimport itertools\n\nclass QueueFIFO:\n \"\"\"\n Class: QueueFIFO\n Description: QueueFI"
},
{
"path": "Search_based_Planning/Search_3D/utils3D.py",
"chars": 13021,
"preview": "import numpy as np\nimport pyrr\nfrom collections import defaultdict\nimport copy\n\n\ndef getRay(x, y):\n direc = [y[0] - x"
}
]
About this extraction
This page contains the full source code of the zhm-real/PathPlanning GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 75 files (389.8 KB), approximately 114.2k tokens, and a symbol index with 844 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.