Merge branch 'master' of https://github.com/zhm-real/path-planning-al…

…gorithms

CurvesGenerator/cubic_spline.py

@@ -25,7 +25,7 @@ def __init__(self, x, y):
         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 @@ def calc(self, t):
         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 @@ def calcd(self, t):
         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 @@ def __init__(self, lengths, ctypes, L, x, y, yaw, directions):
         self.lengths = lengths  # lengths of each part of path (+: forward, -: backward) [float]
         self.ctypes = ctypes  # type of each part of the path [string]
         self.L = L  # total path length [float]
-        self.x = x  # final x positions [m]
+        self.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 @@ def on_press(self, event):
             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 @@ def on_press(self, event):
             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 @@ def empty(self):
         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

@@ -232,7 +232,7 @@ def visualization(self):
             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 @@
 # 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 @@ def move_start(self, x):
     def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], theta = [0,0,0], block_to_move = 0, obb_to_move = 0, mode = 'uniform'):
         # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, 
         # R is an orthorgonal transform in R3*3, is the rotation matrix
-        # (x',t') = (x + tv, t) is uniform transformation
+        # (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 @@ def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], theta = [0,0,0], block_t
                     # 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 @@ def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], theta = [0,0,0], block_t
                     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 @@ def __init__(self):
         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/rrt_star3D.py

@@ -38,7 +38,7 @@ def __init__(self):
         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 @@ def on_press(self, event):
         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":