yue qi

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -5,10 +5,10 @@ This is rrt star code for 3D
 """
 import numpy as np
 from numpy.matlib import repmat
-from env3D import env
+from rrt_3D.env3D import env
 from collections import defaultdict
 import pyrr as pyrr
-from utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
 import time
 
 
@@ -17,14 +17,14 @@ class rrtstar():
         self.env = env()
         self.Parent = defaultdict(lambda: defaultdict(dict))
         self.V = []
-        self.E = []
+        self.E = edgeset()
         self.i = 0
         self.maxiter = 10000
         self.stepsize = 0.5
         self.Path = []
 
     def wireup(self,x,y):
-        self.E.append([x,y]) # add edge
+        self.E.add_edge([x,y]) # add edge
         self.Parent[str(x[0])][str(x[1])][str(x[2])] = y
 
     def run(self):

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -5,10 +5,10 @@ This is rrt star code for 3D
 """
 import numpy as np
 from numpy.matlib import repmat
-from env3D import env
+from rrt_3D.env3D import env
 from collections import defaultdict
 import pyrr as pyrr
-from utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
 import time
 
 
@@ -16,21 +16,21 @@ class rrtstar():
     def __init__(self):
         self.env = env()
         self.Parent = defaultdict(lambda: defaultdict(dict))
+        self.E = edgeset()
         self.V = []
-        self.E = []
         self.i = 0
         self.maxiter = 10000
         self.stepsize = 0.5
         self.Path = []
 
     def wireup(self,x,y):
-        self.E.append([x,y]) # add edge
+        self.E.add_edge([x,y]) # add edge
         self.Parent[str(x[0])][str(x[1])][str(x[2])] = y
 
     def removewire(self,xnear):
         xparent = self.Parent[str(xnear[0])][str(xnear[1])][str(xnear[2])]
-        a = np.array([xnear,xparent])
-        self.E = [xx for xx in self.E if not (xx==a).all()] # remove and replace old the connection
+        a = [xnear,xparent]
+        self.E.remove_edge(a) # remove and replace old the connection
 
     def run(self):
         self.V.append(self.env.start)

Sampling-based Planning/rrt_3D/utils3D.py

@@ -64,11 +64,15 @@ def draw_block_list(ax, blocks):
 
 
 def sampleFree(initparams):
+    '''biased sampling'''
     x = np.random.uniform(initparams.env.boundary[0:3], initparams.env.boundary[3:6])
+    i = np.random.random()
     if isinside(initparams, x):
         return sampleFree(initparams)
     else:
-        return np.array(x)
+        if i < 0.05:
+            return initparams.env.goal
+        else: return np.array(x)
 
 
 def isinside(initparams, x):
@@ -107,8 +111,9 @@ def steer(initparams, x, y):
     return xnew
 
 
-def near(initparams, x, r=2):
+def near(initparams, x):
     # TODO: r = min{gamma*log(card(V)/card(V)1/d),eta}
+    r = initparams.stepsize
     V = np.array(initparams.V)
     if initparams.i == 0:
         return initparams.V[0]
@@ -128,7 +133,7 @@ def cost(initparams, x):
 
 def visualization(initparams):
     V = np.array(initparams.V)
-    E = np.array(initparams.E)
+    E = initparams.E
     Path = np.array(initparams.Path)
     start = initparams.env.start
     goal = initparams.env.goal
@@ -136,8 +141,9 @@ def visualization(initparams):
     ax.view_init(elev=0., azim=90)
     ax.clear()
     draw_block_list(ax, initparams.env.blocks)
-    if E != []:
-        for i in E:
+    edges = E.get_edge()
+    if edges != []:
+        for i in edges:
             xs = i[0][0], i[1][0]
             ys = i[0][1], i[1][1]
             zs = i[0][2], i[1][2]
@@ -175,3 +181,40 @@ def path(initparams, Path=[], dist=0):
         x = x2
     return Path, dist
 
+class edgeset(object):
+    def __init__(self):
+        self.E = {}
+
+    def hash(self,x):
+        return str(x[0])+' '+str(x[1])+' '+str(x[2])
+
+    def dehash(self,x):
+        return np.array([float(i) for i in x.split(' ')])
+
+    def add_edge(self,edge):
+        x, y = self.hash(edge[0]),self.hash(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]
+        self.E[self.hash(x)].remove(self.hash(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((self.dehash(v),self.dehash(n)))
+        return edges
+
+if __name__ == '__main__':
+    x = np.array([1.0,2.3,3.3])
+    y = np.array([1.2,2.4,3.5])
+    E = edgeset()
+    E.add_edge([x,y])
+    print(E.get_edge())
+    E.remove_edge([x,y])
+    print(E.get_edge())