diff --git a/.idea/PathPlanning.iml b/.idea/PathPlanning.iml
index 49df05d..a257e8c 100644
--- a/.idea/PathPlanning.iml
+++ b/.idea/PathPlanning.iml
@@ -2,10 +2,11 @@
-
+
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
index 8161a60..a2e120d 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -1,4 +1,4 @@
-
+
\ No newline at end of file
diff --git a/CurvesGenerator/cubic_spline.py b/CurvesGenerator/cubic_spline.py
index d33b461..c7504ef 100644
--- a/CurvesGenerator/cubic_spline.py
+++ b/CurvesGenerator/cubic_spline.py
@@ -25,7 +25,7 @@ class Spline:
self.x = x
self.y = y
- self.nx = len(x) # dimension of x
+ self.nx = len(x) # dimension of s
h = np.diff(x)
# calc coefficient cBest
@@ -48,7 +48,7 @@ class Spline:
u"""
Calc position
- if t is outside of the input x, return None
+ if t is outside of the input s, return None
"""
@@ -68,7 +68,7 @@ class Spline:
u"""
Calc first derivative
- if t is outside of the input x, return None
+ if t is outside of the input s, return None
"""
if t < self.x[0]:
@@ -219,7 +219,7 @@ def test_spline2d():
plt.plot(rx, ry, "-r", label="spline")
plt.grid(True)
plt.axis("equal")
- plt.xlabel("x[m]")
+ plt.xlabel("s[m]")
plt.ylabel("y[m]")
plt.legend()
diff --git a/CurvesGenerator/dubins_path.py b/CurvesGenerator/dubins_path.py
index 33361fe..94ecf47 100644
--- a/CurvesGenerator/dubins_path.py
+++ b/CurvesGenerator/dubins_path.py
@@ -14,7 +14,7 @@ class PATH:
def __init__(self, L, mode, x, y, yaw):
self.L = L # total path length [float]
self.mode = mode # type of each part of the path [string]
- self.x = x # final x positions [m]
+ self.x = x # final s positions [m]
self.y = y # final y positions [m]
self.yaw = yaw # final yaw angles [rad]
@@ -298,7 +298,7 @@ def calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, curv, step_size=0.1):
def main():
- # choose states pairs: (x, y, yaw)
+ # 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)]
diff --git a/CurvesGenerator/reeds_shepp.py b/CurvesGenerator/reeds_shepp.py
index 175def2..f5c18b5 100644
--- a/CurvesGenerator/reeds_shepp.py
+++ b/CurvesGenerator/reeds_shepp.py
@@ -16,7 +16,7 @@ class PATH:
self.lengths = lengths # lengths of each part of path (+: forward, -: backward) [float]
self.ctypes = ctypes # type of each part of the path [string]
self.L = L # total path length [float]
- self.x = x # final x positions [m]
+ self.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
@@ -573,7 +573,7 @@ def pi_2_pi(theta):
def R(x, y):
"""
- Return the polar coordinates (r, theta) of the point (x, y)
+ Return the polar coordinates (r, theta) of the point (s, y)
"""
r = math.hypot(x, y)
theta = math.atan2(y, x)
@@ -667,7 +667,7 @@ def check_path(sx, sy, syaw, gx, gy, gyaw, maxc):
def main():
- # choose states pairs: (x, y, yaw)
+ # 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)]
diff --git a/README.md b/README.md
index 83b43d5..33ba91f 100644
--- a/README.md
+++ b/README.md
@@ -117,7 +117,7 @@ Directory Structure
## Papers
### Search-base Planning
-* [A*: ](https://ieeexplore.ieee.org/document/4082128) A Formal Basis for the Heuristic Determination of Minimum Cost Paths
+* [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*
@@ -142,7 +142,7 @@ Directory Structure
* [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
+* [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
diff --git a/Sampling_based_Planning/rrt_2D/dynamic_rrt.py b/Sampling_based_Planning/rrt_2D/dynamic_rrt.py
index e365971..82f22e8 100644
--- a/Sampling_based_Planning/rrt_2D/dynamic_rrt.py
+++ b/Sampling_based_Planning/rrt_2D/dynamic_rrt.py
@@ -93,7 +93,7 @@ class DynamicRrt:
print("Please choose right area!")
else:
x, y = int(x), int(y)
- print("Add circle obstacle at: x =", x, ",", "y =", 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)
diff --git a/Sampling_based_Planning/rrt_2D/extended_rrt.py b/Sampling_based_Planning/rrt_2D/extended_rrt.py
index 746e365..11d994e 100644
--- a/Sampling_based_Planning/rrt_2D/extended_rrt.py
+++ b/Sampling_based_Planning/rrt_2D/extended_rrt.py
@@ -79,7 +79,7 @@ class ExtendedRrt:
print("Please choose right area!")
else:
x, y = int(x), int(y)
- print("Add circle obstacle at: x =", x, ",", "y =", 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()
diff --git a/Sampling_based_Planning/rrt_2D/queue.py b/Sampling_based_Planning/rrt_2D/queue.py
index 8f481ae..51703ae 100644
--- a/Sampling_based_Planning/rrt_2D/queue.py
+++ b/Sampling_based_Planning/rrt_2D/queue.py
@@ -53,7 +53,7 @@ class QueuePrior:
return len(self.queue) == 0
def put(self, item, priority):
- heapq.heappush(self.queue, (priority, item)) # reorder x using 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
diff --git a/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py b/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py
index 90404e6..a8a09d7 100644
--- a/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py
+++ b/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py
@@ -232,7 +232,7 @@ class dynamic_rrt_3D:
dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
make_transparent(ax)
- # plt.xlabel('x')
+ # plt.xlabel('s')
# plt.ylabel('y')
ax.set_axis_off()
plt.pause(0.0001)
diff --git a/Sampling_based_Planning/rrt_3D/env3D.py b/Sampling_based_Planning/rrt_3D/env3D.py
index 3235003..9f6bca5 100644
--- a/Sampling_based_Planning/rrt_3D/env3D.py
+++ b/Sampling_based_Planning/rrt_3D/env3D.py
@@ -8,7 +8,7 @@ import numpy as np
# from utils3D import OBB2AABB
def R_matrix(z_angle,y_angle,x_angle):
- # x angle: row; y angle: pitch; z angle: yaw
+ # 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
@@ -107,7 +107,7 @@ class env():
def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], theta = [0,0,0], block_to_move = 0, obb_to_move = 0, mode = 'uniform'):
# t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time,
# R is an orthorgonal transform in R3*3, is the rotation matrix
- # (x',t') = (x + tv, t) is uniform transformation
+ # (s',t') = (s + tv, t) is uniform transformation
if mode == 'uniform':
ori = np.array(self.blocks[block_to_move])
self.blocks[block_to_move] = \
@@ -129,7 +129,7 @@ class env():
# np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \
# ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
return a,ori
- # (x',t') = (x + a, t + s) is a translation
+ # (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] = \
@@ -152,7 +152,7 @@ class env():
np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \
ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
# return a,ori
- # (x',t') = (Rx, t)
+ # (s',t') = (Rx, t)
if mode == 'rotation': # this makes an OBB rotate
ori = [self.OBB[obb_to_move]]
self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2])
diff --git a/Sampling_based_Planning/rrt_3D/plot_util3D.py b/Sampling_based_Planning/rrt_3D/plot_util3D.py
index 1c4cb33..d4488da 100644
--- a/Sampling_based_Planning/rrt_3D/plot_util3D.py
+++ b/Sampling_based_Planning/rrt_3D/plot_util3D.py
@@ -128,7 +128,7 @@ def visualization(initparams):
dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
make_transparent(ax)
- #plt.xlabel('x')
+ #plt.xlabel('s')
#plt.ylabel('y')
ax.set_axis_off()
plt.pause(0.0001)
diff --git a/Sampling_based_Planning/rrt_3D/rrt3D.py b/Sampling_based_Planning/rrt_3D/rrt3D.py
index 9c5444f..f2ee776 100644
--- a/Sampling_based_Planning/rrt_3D/rrt3D.py
+++ b/Sampling_based_Planning/rrt_3D/rrt3D.py
@@ -36,7 +36,7 @@ class rrt():
self.fig = plt.figure(figsize=(10, 8))
def wireup(self, x, y):
- # self.E.add_edge([x, y]) # add edge
+ # self.E.add_edge([s, y]) # add edge
self.Parent[x] = y
def run(self):
diff --git a/Sampling_based_Planning/rrt_3D/rrt_star3D.py b/Sampling_based_Planning/rrt_3D/rrt_star3D.py
index ba5b5ed..b94aed7 100644
--- a/Sampling_based_Planning/rrt_3D/rrt_star3D.py
+++ b/Sampling_based_Planning/rrt_3D/rrt_star3D.py
@@ -38,7 +38,7 @@ class rrtstar():
self.V.append(self.x0)
self.ind = 0
def wireup(self,x,y):
- # self.E.add_edge([x,y]) # add edge
+ # self.E.add_edge([s,y]) # add edge
self.Parent[x] = y
def removewire(self,xnear):
diff --git a/Sampling_based_Planning/rrt_3D/utils3D.py b/Sampling_based_Planning/rrt_3D/utils3D.py
index 0214f0c..16e56ce 100644
--- a/Sampling_based_Planning/rrt_3D/utils3D.py
+++ b/Sampling_based_Planning/rrt_3D/utils3D.py
@@ -121,7 +121,7 @@ def lineAABB(p0, p1, dist, aabb):
if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False
if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False
if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False
- # I.cross(x axis) ?
+ # I.cross(s axis) ?
r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1])
if abs(T[1] * I[2] - T[2] * I[1]) > r: return False
# I.cross(y axis) ?
@@ -176,7 +176,7 @@ def nearest(initparams, x, isset=False):
return tuple(initparams.V[np.argmin(dists)])
def near(initparams, x):
- # x = np.array(x)
+ # s = np.array(s)
V = np.array(initparams.V)
if initparams.i == 0:
return [initparams.V[0]]
@@ -192,15 +192,15 @@ def near(initparams, x):
return np.array(nearpoints)
def steer(initparams, x, y, DIST=False):
- # steer from x to y
+ # steer from s to y
if np.equal(x, y).all():
return x, 0.0
dist, step = getDist(y, x), initparams.stepsize
step = min(dist, step)
increment = ((y[0] - x[0]) / dist * step, (y[1] - x[1]) / dist * step, (y[2] - x[2]) / dist * step)
xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2])
- # direc = (y - x) / np.linalg.norm(y - x)
- # xnew = x + initparams.stepsize * direc
+ # direc = (y - s) / np.linalg.norm(y - s)
+ # xnew = s + initparams.stepsize * direc
if DIST:
return xnew, dist
return xnew, dist
@@ -274,7 +274,7 @@ def tree_add_edge(node_in_tree, x):
return node_to_add
def tree_bfs(head, x):
- # searches x in order of bfs
+ # searches s in order of bfs
node = head
Q = []
Q.append(node)
@@ -286,7 +286,7 @@ def tree_bfs(head, x):
Q.append(child_node)
def tree_nearest(head, x):
- # find the node nearest to x
+ # find the node nearest to s
D = np.inf
min_node = None
@@ -304,7 +304,7 @@ def tree_nearest(head, x):
return min_node
def tree_steer(initparams, node, x):
- # steer from node to x
+ # steer from node to s
dist, step = getDist(node.pos, x), initparams.stepsize
increment = ((node.pos[0] - x[0]) / dist * step, (node.pos[1] - x[1]) / dist * step, (node.pos[2] - x[2]) / dist * step)
xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2])
diff --git a/Search_based_Planning/Search_2D/Anytime_D_star.py b/Search_based_Planning/Search_2D/Anytime_D_star.py
index 7d0b093..76b3944 100644
--- a/Search_based_Planning/Search_2D/Anytime_D_star.py
+++ b/Search_based_Planning/Search_2D/Anytime_D_star.py
@@ -79,7 +79,7 @@ class ADStar:
else:
self.count_env_change += 1
x, y = int(x), int(y)
- print("Change position: x =", x, ",", "y =", y)
+ print("Change position: s =", x, ",", "y =", y)
# for small changes
if self.title == "Anytime D*: Small changes":
diff --git a/Search_based_Planning/Search_2D/Astar.py b/Search_based_Planning/Search_2D/Astar.py
index 54b5919..d3dacd6 100644
--- a/Search_based_Planning/Search_2D/Astar.py
+++ b/Search_based_Planning/Search_2D/Astar.py
@@ -6,18 +6,18 @@ A_star 2D
import os
import sys
import math
+import heapq
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search_based_Planning/")
-from Search_2D import queue
-from Search_2D import plotting
-from Search_2D import env
+from Search_based_Planning.Search_2D import plotting, env
-class Astar:
- def __init__(self, start, goal, heuristic_type):
- self.s_start, self.s_goal = start, goal
+class AStar:
+ def __init__(self, s_start, s_goal, heuristic_type):
+ self.s_start = s_start
+ self.s_goal = s_goal
self.heuristic_type = heuristic_type
self.Env = env.Env() # class Env
@@ -25,20 +25,25 @@ class Astar:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
- self.g = {self.s_start: 0, self.s_goal: float("inf")} # Cost to come
- self.OPEN = queue.QueuePrior() # priority queue / OPEN set
- self.OPEN.put(self.s_start, self.fvalue(self.s_start))
+ self.OPEN = [] # priority queue / OPEN set
self.CLOSED = [] # CLOSED set / VISITED order
- self.PARENT = {self.s_start: self.s_start}
+ self.PARENT = dict() # recorded parent
+ self.g = dict() # cost to come
def searching(self):
"""
A_star Searching.
- :return: path, order of visited nodes
+ :return: path, visited order
"""
+ self.PARENT[self.s_start] = self.s_start
+ self.g[self.s_start] = 0
+ self.g[self.s_goal] = math.inf
+ heapq.heappush(self.OPEN,
+ (self.f_value(self.s_start), self.s_start))
+
while self.OPEN:
- s = self.OPEN.get()
+ _, s = heapq.heappop(self.OPEN)
self.CLOSED.append(s)
if s == self.s_goal: # stop condition
@@ -46,19 +51,21 @@ class Astar:
for s_n in self.get_neighbor(s):
new_cost = self.g[s] + self.cost(s, s_n)
+
if s_n not in self.g:
- self.g[s_n] = float("inf")
- if new_cost < self.g[s_n]: # conditions for updating Cost
+ self.g[s_n] = math.inf
+
+ if new_cost < self.g[s_n]: # conditions for updating Cost
self.g[s_n] = new_cost
self.PARENT[s_n] = s
- self.OPEN.put(s_n, self.fvalue(s_n))
+ heapq.heappush(self.OPEN, (self.f_value(s_n), s_n))
return self.extract_path(self.PARENT), self.CLOSED
- def repeated_astar(self, e):
+ def searching_repeated_astar(self, e):
"""
- repeated a*.
- :param e: weight of a*
+ repeated A*.
+ :param e: weight of A*
:return: path and visited order
"""
@@ -74,7 +81,7 @@ class Astar:
def repeated_searching(self, s_start, s_goal, e):
"""
- run a* with weight e.
+ run A* with weight e.
:param s_start: starting state
:param s_goal: goal state
:param e: weight of a*
@@ -82,27 +89,29 @@ class Astar:
"""
g = {s_start: 0, s_goal: float("inf")}
- OPEN = queue.QueuePrior()
- OPEN.put(s_start, g[s_start] + e * self.Heuristic(s_start))
- CLOSED = []
PARENT = {s_start: s_start}
+ OPEN = []
+ CLOSED = []
+ heapq.heappush(OPEN,
+ (g[s_start] + e * self.heuristic(s_start), s_start))
while OPEN:
- s = OPEN.get()
+ _, s = heapq.heappop(OPEN)
CLOSED.append(s)
if s == s_goal:
break
for s_n in self.get_neighbor(s):
- if s_n not in CLOSED:
- new_cost = g[s] + self.cost(s, s_n)
- if s_n not in g:
- g[s_n] = float("inf")
- if new_cost < g[s_n]: # conditions for updating Cost
- g[s_n] = new_cost
- PARENT[s_n] = s
- OPEN.put(s_n, g[s_n] + e * self.Heuristic(s_n))
+ new_cost = g[s] + self.cost(s, s_n)
+
+ if s_n not in g:
+ g[s_n] = math.inf
+
+ if new_cost < g[s_n]: # conditions for updating Cost
+ g[s_n] = new_cost
+ PARENT[s_n] = s
+ heapq.heappush(OPEN, (g[s_n] + e * self.heuristic(s_n), s_n))
return self.extract_path(PARENT), CLOSED
@@ -113,12 +122,7 @@ class Astar:
:return: neighbors
"""
- s_list = []
-
- for u in self.u_set:
- s_list.append(tuple([s[i] + u[i] for i in range(2)]))
-
- return s_list
+ return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]
def cost(self, s_start, s_goal):
"""
@@ -130,11 +134,18 @@ class Astar:
"""
if self.is_collision(s_start, s_goal):
- return float("inf")
+ return math.inf
return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
def is_collision(self, s_start, s_end):
+ """
+ check if the line segment (s_start, s_end) is collision.
+ :param s_start: start node
+ :param s_end: end node
+ :return: True: is collision / False: not collision
+ """
+
if s_start in self.obs or s_end in self.obs:
return True
@@ -151,14 +162,14 @@ class Astar:
return False
- def fvalue(self, x):
+ def f_value(self, s):
"""
- f = g + h. (g: Cost to come, h: heuristic function)
- :param x: current state
+ f = g + h. (g: Cost to come, h: heuristic value)
+ :param s: current state
:return: f
"""
- return self.g[x] + self.Heuristic(x)
+ return self.g[s] + self.heuristic(s)
def extract_path(self, PARENT):
"""
@@ -178,7 +189,7 @@ class Astar:
return list(path)
- def Heuristic(self, s):
+ def heuristic(self, s):
"""
Calculate heuristic.
:param s: current node (state)
@@ -198,13 +209,13 @@ def main():
s_start = (5, 5)
s_goal = (45, 25)
- astar = Astar(s_start, s_goal, "euclidean")
+ astar = AStar(s_start, s_goal, "euclidean")
plot = plotting.Plotting(s_start, s_goal)
path, visited = astar.searching()
plot.animation(path, visited, "A*") # animation
- # path, visited = astar.repeated_astar(2.5) # initial weight e = 2.5
+ # path, visited = astar.searching_repeated_astar(2.5) # initial weight e = 2.5
# plot.animation_ara_star(path, visited, "Repeated A*")
diff --git a/Search_based_Planning/Search_2D/Best_First.py b/Search_based_Planning/Search_2D/Best_First.py
index 8c6b4c3..c820a0a 100644
--- a/Search_based_Planning/Search_2D/Best_First.py
+++ b/Search_based_Planning/Search_2D/Best_First.py
@@ -6,18 +6,18 @@ Best-First Searching
import os
import sys
import math
+import heapq
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search_based_Planning/")
-from Search_2D import queue
-from Search_2D import plotting
-from Search_2D import env
+from Search_based_Planning.Search_2D import plotting, env
class BestFirst:
def __init__(self, s_start, s_goal):
- self.s_start, self.s_goal = s_start, s_goal
+ self.s_start = s_start
+ self.s_goal = s_goal
self.Env = env.Env()
self.plotting = plotting.Plotting(self.s_start, self.s_goal)
@@ -25,10 +25,9 @@ class BestFirst:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
- self.OPEN = queue.QueuePrior() # OPEN set
- self.OPEN.put(self.s_start, self.Heuristic(self.s_start))
+ self.OPEN = [] # OPEN set: visited nodes
self.CLOSED = [] # CLOSED set / visited order
- self.PARENT = {self.s_start: self.s_start}
+ self.PARENT = dict() # recorded parent
def searching(self):
"""
@@ -36,30 +35,35 @@ class BestFirst:
:return: planning path, visited order
"""
+ self.PARENT[self.s_start] = self.s_start
+ heapq.heappush(self.OPEN,
+ (self.heuristic(self.s_start), self.s_start))
+
while self.OPEN:
- s = self.OPEN.get()
+ _, s = heapq.heappop(self.OPEN)
if s == self.s_goal:
break
self.CLOSED.append(s)
for s_n in self.get_neighbor(s):
+ if self.is_collision(s, s_n):
+ continue
+
if s_n not in self.PARENT: # node not explored
- self.OPEN.put(s_n, self.Heuristic(s_n))
+ heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n))
self.PARENT[s_n] = s
return self.extract_path(), self.CLOSED
- def Heuristic(self, s):
+ def heuristic(self, s):
"""
estimated distance between current state and goal state.
:param s: current state
- :return: estimated distance
+ :return: Euclidean distance
"""
- h = math.hypot(s[0] - self.s_goal[0], s[1] - self.s_goal[1])
-
- return h
+ return math.hypot(s[0] - self.s_goal[0], s[1] - self.s_goal[1])
def get_neighbor(self, s):
"""
@@ -68,16 +72,16 @@ class BestFirst:
:return: neighbors
"""
- s_list = []
-
- for u in self.u_set:
- s_next = tuple([s[i] + u[i] for i in range(2)])
- if not self.is_collision(s, s_next):
- s_list.append(s_next)
-
- return s_list
+ return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]
def is_collision(self, s_start, s_end):
+ """
+ check if the line segment (s_start, s_end) is collision.
+ :param s_start: start node
+ :param s_end: end node
+ :return: True: is collision / False: not collision
+ """
+
if s_start in self.obs or s_end in self.obs:
return True
diff --git a/Search_based_Planning/Search_2D/Bidirectional_a_star.py b/Search_based_Planning/Search_2D/Bidirectional_a_star.py
index 692dc5e..604c410 100644
--- a/Search_based_Planning/Search_2D/Bidirectional_a_star.py
+++ b/Search_based_Planning/Search_2D/Bidirectional_a_star.py
@@ -6,18 +6,18 @@ Bidirectional_a_star 2D
import os
import sys
import math
+import heapq
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search_based_Planning/")
-from Search_2D import queue
-from Search_2D import plotting
-from Search_2D import env
+from Search_based_Planning.Search_2D import plotting, env
-class BidirectionalAstar:
+class BidirectionalAStar:
def __init__(self, s_start, s_goal, heuristic_type):
- self.s_start, self.s_goal = s_start, s_goal
+ self.s_start = s_start
+ self.s_goal = s_goal
self.heuristic_type = heuristic_type
self.Env = env.Env() # class Env
@@ -25,58 +25,82 @@ class BidirectionalAstar:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
- self.g_fore = {self.s_start: 0, self.s_goal: float("inf")} # Cost to come: from x_init
- self.g_back = {self.s_goal: 0, self.s_start: float("inf")} # Cost to come: form x_goal
-
- self.OPEN_fore = queue.QueuePrior() # OPEN set for foreward searching
- self.OPEN_fore.put(self.s_start,
- self.g_fore[self.s_start] + self.h(self.s_start, self.s_goal))
- self.OPEN_back = queue.QueuePrior() # OPEN set for backward searching
- self.OPEN_back.put(self.s_goal,
- self.g_back[self.s_goal] + self.h(self.s_goal, self.s_start))
-
- self.CLOSED_fore = [] # CLOSED set for foreward
+ self.OPEN_fore = [] # OPEN set for forward searching
+ self.OPEN_back = [] # OPEN set for backward searching
+ self.CLOSED_fore = [] # CLOSED set for forward
self.CLOSED_back = [] # CLOSED set for backward
+ self.PARENT_fore = dict() # recorded parent for forward
+ self.PARENT_back = dict() # recorded parent for backward
+ self.g_fore = dict() # cost to come for forward
+ self.g_back = dict() # cost to come for backward
- self.PARENT_fore = {self.s_start: self.s_start}
- self.PARENT_back = {self.s_goal: self.s_goal}
+ def init(self):
+ """
+ initialize parameters
+ """
+
+ self.g_fore[self.s_start] = 0.0
+ self.g_fore[self.s_goal] = math.inf
+ self.g_back[self.s_goal] = 0.0
+ self.g_back[self.s_start] = math.inf
+ self.PARENT_fore[self.s_start] = self.s_start
+ self.PARENT_back[self.s_goal] = self.s_goal
+ heapq.heappush(self.OPEN_fore,
+ (self.f_value_fore(self.s_start), self.s_start))
+ heapq.heappush(self.OPEN_back,
+ (self.f_value_back(self.s_goal), self.s_goal))
def searching(self):
+ """
+ Bidirectional A*
+ :return: connected path, visited order of forward, visited order of backward
+ """
+
+ self.init()
s_meet = self.s_start
while self.OPEN_fore and self.OPEN_back:
# solve foreward-search
- s_fore = self.OPEN_fore.get()
+ _, s_fore = heapq.heappop(self.OPEN_fore)
if s_fore in self.PARENT_back:
s_meet = s_fore
break
+
self.CLOSED_fore.append(s_fore)
for s_n in self.get_neighbor(s_fore):
new_cost = self.g_fore[s_fore] + self.cost(s_fore, s_n)
+
if s_n not in self.g_fore:
- self.g_fore[s_n] = float("inf")
+ self.g_fore[s_n] = math.inf
+
if new_cost < self.g_fore[s_n]:
self.g_fore[s_n] = new_cost
self.PARENT_fore[s_n] = s_fore
- self.OPEN_fore.put(s_n, new_cost + self.h(s_n, self.s_goal))
+ heapq.heappush(self.OPEN_fore,
+ (self.f_value_fore(s_n), s_n))
# solve backward-search
- s_back = self.OPEN_back.get()
+ _, s_back = heapq.heappop(self.OPEN_back)
+
if s_back in self.PARENT_fore:
s_meet = s_back
break
+
self.CLOSED_back.append(s_back)
for s_n in self.get_neighbor(s_back):
new_cost = self.g_back[s_back] + self.cost(s_back, s_n)
+
if s_n not in self.g_back:
- self.g_back[s_n] = float("inf")
+ self.g_back[s_n] = math.inf
+
if new_cost < self.g_back[s_n]:
self.g_back[s_n] = new_cost
self.PARENT_back[s_n] = s_back
- self.OPEN_back.put(s_n, new_cost + self.h(s_n, self.s_start))
+ heapq.heappush(self.OPEN_back,
+ (self.f_value_back(s_n), s_n))
return self.extract_path(s_meet), self.CLOSED_fore, self.CLOSED_back
@@ -87,14 +111,7 @@ class BidirectionalAstar:
:return: neighbors
"""
- s_list = set()
-
- for u in self.u_set:
- s_next = tuple([s[i] + u[i] for i in range(2)])
- if s_next not in self.obs:
- s_list.add(s_next)
-
- return s_list
+ return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]
def extract_path(self, s_meet):
"""
@@ -125,6 +142,24 @@ class BidirectionalAstar:
return list(reversed(path_fore)) + list(path_back)
+ def f_value_fore(self, s):
+ """
+ forward searching: f = g + h. (g: Cost to come, h: heuristic value)
+ :param s: current state
+ :return: f
+ """
+
+ return self.g_fore[s] + self.h(s, self.s_goal)
+
+ def f_value_back(self, s):
+ """
+ backward searching: f = g + h. (g: Cost to come, h: heuristic value)
+ :param s: current state
+ :return: f
+ """
+
+ return self.g_back[s] + self.h(s, self.s_start)
+
def h(self, s, goal):
"""
Calculate heuristic value.
@@ -150,11 +185,18 @@ class BidirectionalAstar:
"""
if self.is_collision(s_start, s_goal):
- return float("inf")
+ return math.inf
return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
def is_collision(self, s_start, s_end):
+ """
+ check if the line segment (s_start, s_end) is collision.
+ :param s_start: start node
+ :param s_end: end node
+ :return: True: is collision / False: not collision
+ """
+
if s_start in self.obs or s_end in self.obs:
return True
@@ -176,7 +218,7 @@ def main():
x_start = (5, 5)
x_goal = (45, 25)
- bastar = BidirectionalAstar(x_start, x_goal, "euclidean")
+ bastar = BidirectionalAStar(x_start, x_goal, "euclidean")
plot = plotting.Plotting(x_start, x_goal)
path, visited_fore, visited_back = bastar.searching()
diff --git a/Search_based_Planning/Search_2D/D_star.py b/Search_based_Planning/Search_2D/D_star.py
index 45bf9b7..6d6f716 100644
--- a/Search_based_Planning/Search_2D/D_star.py
+++ b/Search_based_Planning/Search_2D/D_star.py
@@ -64,7 +64,7 @@ class Dstar:
print("Please choose right area!")
else:
x, y = int(x), int(y)
- print("Add obstacle at: x =", x, ",", "y =", y)
+ print("Add obstacle at: s =", x, ",", "y =", y)
self.obs.add((x, y))
plt.plot(x, y, 'sk')
s = self.s_start
diff --git a/Search_based_Planning/Search_2D/D_star_Lite.py b/Search_based_Planning/Search_2D/D_star_Lite.py
index ed0fe04..80b75f4 100644
--- a/Search_based_Planning/Search_2D/D_star_Lite.py
+++ b/Search_based_Planning/Search_2D/D_star_Lite.py
@@ -55,7 +55,7 @@ class DStar:
print("Please choose right area!")
else:
x, y = int(x), int(y)
- print("Change position: x =", x, ",", "y =", y)
+ print("Change position: s =", x, ",", "y =", y)
s_curr = self.s_start
s_last = self.s_start
diff --git a/Search_based_Planning/Search_2D/Dijkstra.py b/Search_based_Planning/Search_2D/Dijkstra.py
index 0b06239..efa99aa 100644
--- a/Search_based_Planning/Search_2D/Dijkstra.py
+++ b/Search_based_Planning/Search_2D/Dijkstra.py
@@ -6,16 +6,18 @@ Dijkstra 2D
import os
import sys
import math
+import heapq
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search_based_Planning/")
-from Search_based_Planning.Search_2D import queue, plotting, env
+from Search_based_Planning.Search_2D import plotting, env
class Dijkstra:
def __init__(self, s_start, s_goal):
- self.s_start, self.s_goal = s_start, s_goal
+ self.s_start = s_start
+ self.s_goal = s_goal
self.Env = env.Env()
self.plotting = plotting.Plotting(self.s_start, self.s_goal)
@@ -23,20 +25,24 @@ class Dijkstra:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
- self.g = {self.s_start: 0, self.s_goal: float("inf")} # Cost to come
- self.OPEN = queue.QueuePrior() # priority queue / OPEN set
- self.OPEN.put(self.s_start, 0)
+ self.OPEN = [] # priority queue / OPEN set
self.CLOSED = [] # closed set & visited
- self.PARENT = {self.s_start: self.s_start}
+ self.PARENT = dict() # record parent
+ self.g = dict() # Cost to come
def searching(self):
"""
Dijkstra Searching.
- :return: path, order of visited nodes in the planning
+ :return: path, visited order
"""
- while not self.OPEN.empty():
- s = self.OPEN.get()
+ self.PARENT[self.s_start] = self.s_start
+ self.g[self.s_start] = 0
+ self.g[self.s_goal] = math.inf
+ heapq.heappush(self.OPEN, (0, self.s_start))
+
+ while self.OPEN:
+ _, s = heapq.heappop(self.OPEN)
self.CLOSED.append(s)
if s == self.s_goal:
@@ -45,10 +51,10 @@ class Dijkstra:
for s_n in self.get_neighbor(s):
new_cost = self.g[s] + self.cost(s, s_n)
if s_n not in self.g:
- self.g[s_n] = float("inf")
+ self.g[s_n] = math.inf
if new_cost < self.g[s_n]:
self.g[s_n] = new_cost
- self.OPEN.put(s_n, new_cost)
+ heapq.heappush(self.OPEN, (new_cost, s_n))
self.PARENT[s_n] = s
return self.extract_path(), self.CLOSED
@@ -60,12 +66,7 @@ class Dijkstra:
:return: neighbors
"""
- s_list = []
-
- for u in self.u_set:
- s_list.append(tuple([s[i] + u[i] for i in range(2)]))
-
- return s_list
+ return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]
def extract_path(self):
"""
@@ -91,15 +92,21 @@ class Dijkstra:
:param s_start: starting node
:param s_goal: end node
:return: Cost for this motion
- :note: Cost function could be more complicate!
"""
if self.is_collision(s_start, s_goal):
- return float("inf")
+ return math.inf
return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
def is_collision(self, s_start, s_end):
+ """
+ check if the line segment (s_start, s_end) is collision.
+ :param s_start: start node
+ :param s_end: end node
+ :return: True: is collision / False: not collision
+ """
+
if s_start in self.obs or s_end in self.obs:
return True
diff --git a/Search_based_Planning/Search_2D/LPAstar.py b/Search_based_Planning/Search_2D/LPAstar.py
index cb23701..200e820 100644
--- a/Search_based_Planning/Search_2D/LPAstar.py
+++ b/Search_based_Planning/Search_2D/LPAstar.py
@@ -57,7 +57,7 @@ class LpaStar:
print("Please choose right area!")
else:
x, y = int(x), int(y)
- print("Change position: x =", x, ",", "y =", y)
+ print("Change position: s =", x, ",", "y =", y)
self.visited = set()
self.count += 1
if (x, y) not in self.obs:
diff --git a/Search_based_Planning/Search_2D/bfs.py b/Search_based_Planning/Search_2D/bfs.py
index dffa661..315e6d8 100644
--- a/Search_based_Planning/Search_2D/bfs.py
+++ b/Search_based_Planning/Search_2D/bfs.py
@@ -1,22 +1,22 @@
"""
-BFS 2D (Breadth-first Searching)
+Breadth-first Searching_2D (BFS)
@author: huiming zhou
"""
import os
import sys
+from collections import deque
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search_based_Planning/")
-from Search_2D import queue
-from Search_2D import plotting
-from Search_2D import env
+from Search_based_Planning.Search_2D import plotting, env
class BFS:
def __init__(self, s_start, s_goal):
- self.s_start, self.s_goal = s_start, s_goal
+ self.s_start = s_start
+ self.s_goal = s_goal
self.Env = env.Env()
self.plotting = plotting.Plotting(self.s_start, self.s_goal)
@@ -24,10 +24,9 @@ class BFS:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
- self.OPEN = queue.QueueFIFO() # OPEN set: visited nodes
- self.OPEN.put(self.s_start)
+ self.OPEN = deque() # OPEN set: visited nodes
+ self.PARENT = dict() # recorded parent
self.CLOSED = [] # CLOSED set: explored nodes
- self.PARENT = {self.s_start: self.s_start}
def searching(self):
"""
@@ -35,16 +34,21 @@ class BFS:
:return: path, visited order
"""
+ self.PARENT[self.s_start] = self.s_start
+ self.OPEN.append(self.s_start)
+
while self.OPEN:
- s = self.OPEN.get()
+ s = self.OPEN.popleft()
if s == self.s_goal:
break
self.CLOSED.append(s)
for s_n in self.get_neighbor(s):
- if s_n not in self.PARENT: # node not explored
- self.OPEN.put(s_n)
+ if self.is_collision(s, s_n):
+ continue
+ if s_n not in self.PARENT: # node not explored
+ self.OPEN.append(s_n)
self.PARENT[s_n] = s
return self.extract_path(), self.CLOSED
@@ -53,19 +57,19 @@ class BFS:
"""
find neighbors of state s that not in obstacles.
:param s: state
- :return: neighbors
+ :return: neighbors : [nodes]
"""
- s_list = []
-
- for u in self.u_set:
- s_next = tuple([s[i] + u[i] for i in range(2)])
- if not self.is_collision(s, s_next):
- s_list.append(s_next)
-
- return s_list
+ return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]
def is_collision(self, s_start, s_end):
+ """
+ check if the line segment (s_start, s_end) is collision.
+ :param s_start: start node
+ :param s_end: end node
+ :return: True: is collision / False: not collision
+ """
+
if s_start in self.obs or s_end in self.obs:
return True
@@ -85,7 +89,7 @@ class BFS:
def extract_path(self):
"""
Extract the path based on the PARENT set.
- :return: The planning path
+ :return: The planning path : [nodes]
"""
path = [self.s_goal]
@@ -94,6 +98,7 @@ class BFS:
while True:
s = self.PARENT[s]
path.append(s)
+
if s == self.s_start:
break
diff --git a/Search_based_Planning/Search_2D/dfs.py b/Search_based_Planning/Search_2D/dfs.py
index 820680b..f835b9e 100644
--- a/Search_based_Planning/Search_2D/dfs.py
+++ b/Search_based_Planning/Search_2D/dfs.py
@@ -5,18 +5,18 @@ Depth-first Searching_2D (DFS)
import os
import sys
+from collections import deque
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search_based_Planning/")
-from Search_2D import queue
-from Search_2D import plotting
-from Search_2D import env
+from Search_based_Planning.Search_2D import plotting, env
class DFS:
def __init__(self, s_start, s_goal):
- self.s_start, self.s_goal = s_start, s_goal
+ self.s_start = s_start
+ self.s_goal = s_goal
self.Env = env.Env()
self.plotting = plotting.Plotting(self.s_start, self.s_goal)
@@ -24,10 +24,9 @@ class DFS:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
- self.OPEN = queue.QueueLIFO() # OPEN set
- self.OPEN.put(self.s_start)
+ self.OPEN = deque() # OPEN set: visited nodes
+ self.PARENT = dict() # recorded parent
self.CLOSED = [] # CLOSED set / visited order
- self.PARENT = {self.s_start: self.s_start}
def searching(self):
"""
@@ -35,16 +34,21 @@ class DFS:
:return: planning path, visited order
"""
+ self.PARENT[self.s_start] = self.s_start
+ self.OPEN.append(self.s_start)
+
while self.OPEN:
- s = self.OPEN.get()
+ s = self.OPEN.pop()
if s == self.s_goal:
break
self.CLOSED.append(s)
for s_n in self.get_neighbor(s):
+ if self.is_collision(s, s_n):
+ continue
if s_n not in self.PARENT: # node not explored
- self.OPEN.put(s_n)
+ self.OPEN.append(s_n)
self.PARENT[s_n] = s
return self.extract_path(), self.CLOSED
@@ -53,19 +57,19 @@ class DFS:
"""
find neighbors of state s that not in obstacles.
:param s: state
- :return: neighbors
+ :return: neighbors : [nodes]
"""
- s_list = []
-
- for u in self.u_set:
- s_next = tuple([s[i] + u[i] for i in range(2)])
- if not self.is_collision(s, s_next):
- s_list.append(s_next)
-
- return s_list
+ return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]
def is_collision(self, s_start, s_end):
+ """
+ check if the line segment (s_start, s_end) is collision.
+ :param s_start: start node
+ :param s_end: end node
+ :return: True: is collision / False: not collision
+ """
+
if s_start in self.obs or s_end in self.obs:
return True
diff --git a/Search_based_Planning/Search_2D/queue.py b/Search_based_Planning/Search_2D/queue.py
index 8f481ae..51703ae 100644
--- a/Search_based_Planning/Search_2D/queue.py
+++ b/Search_based_Planning/Search_2D/queue.py
@@ -53,7 +53,7 @@ class QueuePrior:
return len(self.queue) == 0
def put(self, item, priority):
- heapq.heappush(self.queue, (priority, item)) # reorder x using 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
diff --git a/Search_based_Planning/Search_3D/Dstar3D.py b/Search_based_Planning/Search_3D/Dstar3D.py
index 9332d4e..523d462 100644
--- a/Search_based_Planning/Search_3D/Dstar3D.py
+++ b/Search_based_Planning/Search_3D/Dstar3D.py
@@ -64,7 +64,7 @@ class D_star(object):
return None, -1
def insert(self, x, h_new):
- # inserting a key and value into OPEN list (x, kx)
+ # inserting a key and value into OPEN list (s, kx)
# depending on following situations
if self.tag[x] == 'New':
kx = h_new
@@ -83,7 +83,7 @@ class D_star(object):
self.V.add(x)
if x is None:
return -1
- # check if 1st timer x
+ # check if 1st timer s
self.checkState(x)
if kold < self.h[x]: # raised states
for y in children(self, x):
diff --git a/Search_based_Planning/Search_3D/env3D.py b/Search_based_Planning/Search_3D/env3D.py
index c8805ab..58090a2 100644
--- a/Search_based_Planning/Search_3D/env3D.py
+++ b/Search_based_Planning/Search_3D/env3D.py
@@ -10,7 +10,7 @@ import numpy as np
# from utils3D import OBB2AABB
def R_matrix(z_angle, y_angle, x_angle):
- # x angle: row; y angle: pitch; z angle: yaw
+ # 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
@@ -120,7 +120,7 @@ class env():
mode='uniform'):
# t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time,
# R is an orthorgonal transform in R3*3, is the rotation matrix
- # (x',t') = (x + tv, t) is uniform transformation
+ # (s',t') = (s + tv, t) is uniform transformation
if mode == 'uniform':
ori = np.array(self.blocks[block_to_move])
self.blocks[block_to_move] = \
@@ -142,7 +142,7 @@ class env():
# np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \
# ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
return a, ori
- # (x',t') = (x + a, t + s) is a translation
+ # (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] = \
@@ -165,7 +165,7 @@ class env():
np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution,
ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
# return a,ori
- # (x',t') = (Rx, t)
+ # (s',t') = (Rx, t)
if mode == 'rotation': # this makes an OBB rotate
ori = [self.OBB[obb_to_move]]
self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0], y_angle=theta[1], x_angle=theta[2])
diff --git a/Search_based_Planning/Search_3D/plot_util3D.py b/Search_based_Planning/Search_3D/plot_util3D.py
index ad4c5f0..afea609 100644
--- a/Search_based_Planning/Search_3D/plot_util3D.py
+++ b/Search_based_Planning/Search_3D/plot_util3D.py
@@ -112,7 +112,7 @@ def visualization(initparams):
zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5]
dx, dy, dz = xmax-xmin, ymax-ymin, zmax-zmin
ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy)
- plt.xlabel('x')
+ plt.xlabel('s')
plt.ylabel('y')
plt.pause(0.0001)
diff --git a/Search_based_Planning/Search_3D/queue.py b/Search_based_Planning/Search_3D/queue.py
index f49ed26..86501dc 100644
--- a/Search_based_Planning/Search_3D/queue.py
+++ b/Search_based_Planning/Search_3D/queue.py
@@ -53,7 +53,7 @@ class QueuePrior:
return len(self.queue) == 0
def put(self, item, priority):
- heapq.heappush(self.queue, (priority, item)) # reorder x using 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
@@ -131,13 +131,13 @@ class MinheapPQ:
# def put(self, item, priority):
# count = 0
-# for (p, x) in self.queue:
-# if x == item:
+# for (p, s) in self.queue:
+# if s == item:
# self.queue[count] = (priority, item)
# break
# count += 1
# if count == len(self.queue):
-# heapq.heappush(self.queue, (priority, item)) # reorder x using 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
@@ -146,9 +146,9 @@ class MinheapPQ:
# return self.queue
# def check_remove(self, item):
-# for (p, x) in self.queue:
-# if item == x:
-# self.queue.remove((p, x))
+# for (p, s) in self.queue:
+# if item == s:
+# self.queue.remove((p, s))
# def top_key(self):
# return self.queue[0][0]
\ No newline at end of file
diff --git a/Search_based_Planning/Search_3D/utils3D.py b/Search_based_Planning/Search_3D/utils3D.py
index 116282f..69cdd8d 100644
--- a/Search_based_Planning/Search_3D/utils3D.py
+++ b/Search_based_Planning/Search_3D/utils3D.py
@@ -72,7 +72,7 @@ def OBB2AABB(obb):
P = obb.P
a = obb.E
A = obb.O
- # a1(A1 dot x) + a2(A2 dot x) + a3(A3 dot x)
+ # a1(A1 dot s) + a2(A2 dot s) + a3(A3 dot s)
Ex = a[0]*abs(A[0][0]) + a[1]*abs(A[1][0]) + a[2]*abs(A[2][0])
Ey = a[0]*abs(A[0][1]) + a[1]*abs(A[1][1]) + a[2]*abs(A[2][1])
Ez = a[0]*abs(A[0][2]) + a[1]*abs(A[1][2]) + a[2]*abs(A[2][2])
@@ -111,7 +111,7 @@ def lineAABB(p0, p1, dist, aabb):
if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False
if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False
if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False
- # I.cross(x axis) ?
+ # I.cross(s axis) ?
r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1])
if abs(T[1] * I[2] - T[2] * I[1]) > r: return False
# I.cross(y axis) ?
@@ -170,63 +170,63 @@ def OBBOBB(obb1, obb2):
return False
#9 cross products
- #L = A0 x B0
+ #L = A0 s B0
ra = a[1]*abs(R[2][0]) + a[2]*abs(R[1][0])
rb = b[1]*abs(R[0][2]) + b[2]*abs(R[0][1])
t = abs(T[2]*R[1][0] - T[1]*R[2][0])
if t > ra + rb:
return False
- #L = A0 x B1
+ #L = A0 s B1
ra = a[1]*abs(R[2][1]) + a[2]*abs(R[1][1])
rb = b[0]*abs(R[0][2]) + b[2]*abs(R[0][0])
t = abs(T[2]*R[1][1] - T[1]*R[2][1])
if t > ra + rb:
return False
- #L = A0 x B2
+ #L = A0 s B2
ra = a[1]*abs(R[2][2]) + a[2]*abs(R[1][2])
rb = b[0]*abs(R[0][1]) + b[1]*abs(R[0][0])
t = abs(T[2]*R[1][2] - T[1]*R[2][2])
if t > ra + rb:
return False
- #L = A1 x B0
+ #L = A1 s B0
ra = a[0]*abs(R[2][0]) + a[2]*abs(R[0][0])
rb = b[1]*abs(R[1][2]) + b[2]*abs(R[1][1])
t = abs( T[0]*R[2][0] - T[2]*R[0][0] )
if t > ra + rb:
return False
- # L = A1 x B1
+ # L = A1 s B1
ra = a[0]*abs(R[2][1]) + a[2]*abs(R[0][1])
rb = b[0]*abs(R[1][2]) + b[2]*abs(R[1][0])
t = abs( T[0]*R[2][1] - T[2]*R[0][1] )
if t > ra + rb:
return False
- #L = A1 x B2
+ #L = A1 s B2
ra = a[0]*abs(R[2][2]) + a[2]*abs(R[0][2])
rb = b[0]*abs(R[1][1]) + b[1]*abs(R[1][0])
t = abs( T[0]*R[2][2] - T[2]*R[0][2] )
if t > ra + rb:
return False
- #L = A2 x B0
+ #L = A2 s B0
ra = a[0]*abs(R[1][0]) + a[1]*abs(R[0][0])
rb = b[1]*abs(R[2][2]) + b[2]*abs(R[2][1])
t = abs( T[1]*R[0][0] - T[0]*R[1][0] )
if t > ra + rb:
return False
- # L = A2 x B1
+ # L = A2 s B1
ra = a[0]*abs(R[1][1]) + a[1]*abs(R[0][1])
rb = b[0] *abs(R[2][2]) + b[2]*abs(R[2][0])
t = abs( T[1]*R[0][1] - T[0]*R[1][1] )
if t > ra + rb:
return False
- #L = A2 x B2
+ #L = A2 s B2
ra = a[0]*abs(R[1][2]) + a[1]*abs(R[0][2])
rb = b[0]*abs(R[2][1]) + b[1]*abs(R[2][0])
t = abs( T[1]*R[0][2] - T[0]*R[1][2] )
@@ -255,7 +255,7 @@ def StateSpace(env, factor=0):
def g_Space(initparams):
'''This function is used to get nodes and discretize the space.
- State space is by x*y*z,3 where each 3 is a point in 3D.'''
+ State space is by s*y*z,3 where each 3 is a point in 3D.'''
g = {}
Space = StateSpace(initparams.env)
for v in Space: