update

Sampling-based Planning/rrt_3D/utils3D.py

@@ -4,13 +4,16 @@ import pyrr as pyrr
 
 import os
 import sys
-sys.path.append(os.path.dirname(os.path.abspath(__file__))+"/../../Sampling-based Planning/")
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
 from rrt_3D.plot_util3D import visualization
 
+
 def getRay(x, y):
     direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]]
     return np.array([x, direc])
 
+
 def getAABB(blocks):
     AABB = []
     for i in blocks:
@@ -21,6 +24,7 @@ def getAABB(blocks):
 def getDist(pos1, pos2):
     return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
 
+
 ''' The following utils can be used for rrt or rrt*,
     required param initparams should have
     env,      environement generated from env3D
@@ -40,9 +44,10 @@ def sampleFree(initparams):
     if isinside(initparams, x):
         return sampleFree(initparams)
     else:
-        if i < 0.05:
-           return initparams.env.goal+0.01
-        else: return np.array(x)
+        if i < 0.1:
+            return initparams.env.goal + 1
+        else:
+            return np.array(x)
         return np.array(x)
 
 
@@ -53,16 +58,18 @@ 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):
+    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)
@@ -98,7 +105,7 @@ def near(initparams, x):
     cardV = len(initparams.V)
     eta = initparams.eta
     gamma = initparams.gamma
-    r = min(gamma*(np.log(cardV)/cardV),eta)
+    r = min(gamma * (np.log(cardV) / cardV), eta)
     if initparams.done: r = 1
     V = np.array(initparams.V)
     if initparams.i == 0:
@@ -126,31 +133,34 @@ def path(initparams, Path=[], dist=0):
         x = x2
     return Path, dist
 
+
 def hash3D(x):
-    return str(x[0])+' '+str(x[1])+' '+str(x[2])
+    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 = {}
 
-    def add_edge(self,edge):
-        x, y = hash3D(edge[0]),hash3D(edge[1])
+    def add_edge(self, edge):
+        x, y = hash3D(edge[0]), hash3D(edge[1])
         if x in self.E:
             self.E[x].append(y)
         else:
             self.E[x] = [y]
 
-    def remove_edge(self,edge):
-        x, y = edge[0],edge[1]
+    def remove_edge(self, edge):
+        x, y = edge[0], edge[1]
         self.E[hash3D(x)].remove(hash3D(y))
 
     def get_edge(self):
         edges = []
         for v in self.E:
             for n in self.E[v]:
-                #if (n,v) not in edges:
-                edges.append((dehash(v),dehash(n)))
-        return edges
+                # if (n,v) not in edges:
+                edges.append((dehash(v), dehash(n)))
+        return edges