'rrtstar'

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -14,8 +14,7 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
 
 from rrt_3D.env3D import env
-from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset, \
-    hash3D, dehash
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
 
 
 class rrtstar():

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -13,7 +13,7 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
 from rrt_3D.env3D import env
-from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset, hash3D, dehash
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
 
 
 class rrtstar():
@@ -23,7 +23,7 @@ class rrtstar():
         self.E = edgeset()
         self.V = []
         self.i = 0
-        self.maxiter = 10000 # at least 4000 in this env
+        self.maxiter = 2000 # at least 2000 in this env
         self.stepsize = 0.5
         self.gamma = 500
         self.eta = 2*self.stepsize
@@ -32,48 +32,54 @@ class rrtstar():
 
     def wireup(self,x,y):
         self.E.add_edge([x,y]) # add edge
-        self.Parent[hash3D(x)] = y
+        self.Parent[x] = y
 
     def removewire(self,xnear):
-        xparent = self.Parent[hash3D(xnear)]
+        xparent = self.Parent[xnear]
         a = [xnear,xparent]
         self.E.remove_edge(a) # remove and replace old the connection
 
     def reached(self):
         self.done = True
+        goal = tuple(self.env.goal)
         xn = near(self,self.env.goal)
-        c = [cost(self,x) for x in xn]
+        c = [cost(self,tuple(x)) for x in xn]
         xncmin = xn[np.argmin(c)]
-        self.wireup(self.env.goal,xncmin)
-        self.V.append(self.env.goal)
+        self.wireup(goal , tuple(xncmin))
+        self.V.append(goal)
         self.Path,self.D = path(self)
 
     def run(self):
-        self.V.append(self.env.start)
+        self.V.append(tuple(self.env.start))
         self.ind = 0
-        xnew = self.env.start
+        xnew = tuple(self.env.start)
         print('start rrt*... ')
         self.fig = plt.figure(figsize = (10,8))
         while self.ind < self.maxiter:
             xrand    = sampleFree(self)
             xnearest = nearest(self,xrand)
             xnew     = steer(self,xnearest,xrand)
-            if not isCollide(self,xnearest,xnew):
+            collide, _ = isCollide(self,xnearest,xnew)
+            if not collide:
                 Xnear = near(self,xnew)
                 self.V.append(xnew) # add point
-                visualization(self)
+                # visualization(self)
                 # minimal path and minimal cost
                 xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew)
                 # connecting along minimal cost path
                 for xnear in Xnear:
+                    xnear = tuple(xnear)
                     c1 = cost(self, xnear) + getDist(xnew, xnear)
-                    if not isCollide(self, xnew, xnear) and c1 < cmin:
+                    collide, _ = isCollide(self, xnew, xnear)
+                    if not collide and c1 < cmin:
                         xmin, cmin = xnear, c1
                 self.wireup(xnew, xmin)
                 # rewire
                 for xnear in Xnear:
+                    xnear = tuple(xnear)
                     c2 = cost(self, xnew) + getDist(xnew, xnear)
-                    if not isCollide(self, xnew, xnear) and c2 < cost(self, xnear):
+                    collide, _ = isCollide(self, xnew, xnear)
+                    if not collide and c2 < cost(self, xnear):
                         self.removewire(xnear)
                         self.wireup(xnear, xnew)
                 self.i += 1

Sampling-based Planning/rrt_3D/utils3D.py

@@ -50,7 +50,7 @@ def sampleFree(initparams):
             return x
         return x
 
-
+# ---------------------- Collision checking algorithms
 def isinside(initparams, x):
     '''see if inside obstacle'''
     for i in initparams.env.blocks:
@@ -58,32 +58,11 @@ def isinside(initparams, x):
             return True
     return False
 
-
 def isinbound(i, x):
     if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]:
         return True
     return False
 
-
-# def isCollide(initparams, x, y):
-#     '''see if line intersects obstacle'''
-#     ray = getRay(x, y)
-#     dist = getDist(x, y)
-#     if not isinbound(initparams.env.boundary, y):
-#         return True
-#     for i in getAABB(initparams.env.blocks):
-#         shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)
-#         if shot is not None:
-#             dist_wall = getDist(x, shot)
-#             if dist_wall <= dist:  # collide
-#                 return True
-#     for i in initparams.env.balls:
-#         shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i)
-#         if shot != []:
-#             dists_ball = [getDist(x, j) for j in shot]
-#             if all(dists_ball <= dist):  # collide
-#                 return True
-#     return False
 def lineSphere(p0, p1, ball):
     # https://cseweb.ucsd.edu/classes/sp19/cse291-d/Files/CSE291_13_CollisionDetection.pdf
     c, r = ball[0:3], ball[-1]
@@ -158,7 +137,7 @@ def isCollide(initparams, x, child, dist=None):
             return True, dist
     return False, dist
 
-
+# ---------------------- leaf node extending algorithms
 def nearest(initparams, x):
     V = np.array(initparams.V)
     if initparams.i == 0:
@@ -167,24 +146,15 @@ def nearest(initparams, x):
     dists = np.linalg.norm(xr - V, axis=1)
     return tuple(initparams.V[np.argmin(dists)])
 
-
-def steer(initparams, x, y):
-    dist, step = getDist(y, x), initparams.stepsize
-    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
-    return xnew
-
-
 def near(initparams, x):
+    x = np.array(x)
+    V = np.array(initparams.V)
     cardV = len(initparams.V)
     eta = initparams.eta
     gamma = initparams.gamma
     r = min(gamma * (np.log(cardV) / cardV), eta)
     if initparams.done: 
         r = 1
-    V = np.array(initparams.V)
     if initparams.i == 0:
         return [initparams.V[0]]
     xr = repmat(x, len(V), 1)
@@ -192,10 +162,17 @@ def near(initparams, x):
     nearpoints = V[inside]
     return np.array(nearpoints)
 
+def steer(initparams, x, y):
+    dist, step = getDist(y, x), initparams.stepsize
+    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
+    return xnew
 
 def cost(initparams, x):
     '''here use the additive recursive cost function'''
-    if all(x == tuple(initparams.env.start)):
+    if x == tuple(initparams.env.start):
         return 0
     xparent = initparams.Parent[x]
     return cost(initparams, xparent) + getDist(x, xparent)
@@ -210,15 +187,6 @@ def path(initparams, Path=[], dist=0):
         x = x2
     return Path, dist
 
-
-def hash3D(x):
-    return str(x[0]) + ' ' + str(x[1]) + ' ' + str(x[2])
-
-
-def dehash(x):
-    return np.array([float(i) for i in x.split(' ')])
-
-
 class edgeset(object):
     def __init__(self):
         self.E = {}