reformat

.idea/PathPlanning.iml

@@ -2,10 +2,11 @@
 <module type="PYTHON_MODULE" version="4">
   <component name="NewModuleRootManager">
     <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="Python 3.7 (base)" jdkType="Python SDK" />
+    <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>

.idea/misc.xml

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

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()
 

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)]

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)]

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

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)

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()

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

Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py

@@ -239,7 +239,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)

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])

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)

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):

Sampling_based_Planning/rrt_3D/rrtstar3D.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):

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])

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":

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*")
 
 

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
 

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
@@ -28,55 +28,82 @@ class BidirectionalAstar:
         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
+
+    def init(self):
+        """
+        initialize parameters
+        """
 
-        self.PARENT_fore = {self.s_start: self.s_start}
-        self.PARENT_back = {self.s_goal: self.s_goal}
+        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 +114,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 +145,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 +188,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 +221,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()

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

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

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
 

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:

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
 

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
 

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

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):

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])

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)
 

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]

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: