changed ball radius

Sampling-based Planning/rrt_3D/plot_util3D.py

@@ -66,7 +66,7 @@ def visualization(initparams):
             xs = i[0][0], i[1][0]
             ys = i[0][1], i[1][1]
             zs = i[0][2], i[1][2]
-            line = plt3d.art3d.Line3D(xs, ys, zs)
+            line = plt3d.art3d.Line3D(xs, ys, zs, alpha=0.25)
             ax.add_line(line)
 
     if Path != []:
@@ -79,7 +79,7 @@ def visualization(initparams):
 
     ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
     ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
-    ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g')
+    ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',)
 
     xmin, xmax = initparams.env.boundary[0], initparams.env.boundary[3]
     ymin, ymax = initparams.env.boundary[1], initparams.env.boundary[4]
@@ -89,7 +89,7 @@ def visualization(initparams):
     ax.set_ylim3d(ymin, ymax)
     ax.set_zlim3d(zmin, zmax)
     ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy)
-    ax.dist = 5
+    #ax.dist = 5
     plt.xlabel('x')
     plt.ylabel('y')
     if not Path != []:
@@ -135,7 +135,7 @@ def make_get_proj(self, rx, ry, rz):
 
         self.eye = E
         self.vvec = R - E
-        self.vvec = self.vvec / proj3d.mod(self.vvec)
+        self.vvec = self.vvec / np.linalg.norm(self.vvec)
 
         if abs(relev) > np.pi/2:
             # upside down

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -12,41 +12,54 @@ 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
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset, hash3D, dehash
 
 
 class rrtstar():
     def __init__(self):
         self.env = env()
-        self.Parent = defaultdict(lambda: defaultdict(dict))
+        self.Parent = {}
         self.E = edgeset()
         self.V = []
         self.i = 0
-        self.maxiter = 10000
+        self.maxiter = 4000 # at least 4000 in this env
         self.stepsize = 0.5
+        self.gamma = 500
+        self.eta = 1.1*self.stepsize
         self.Path = []
+        self.done = False
 
-    def wireup(self, x, y):
-        self.E.add_edge([x, y])  # add edge
-        self.Parent[str(x[0])][str(x[1])][str(x[2])] = y
+    def wireup(self,x,y):
+        self.E.add_edge([x,y]) # add edge
+        self.Parent[hash3D(x)] = y
 
-    def removewire(self, xnear):
-        xparent = self.Parent[str(xnear[0])][str(xnear[1])][str(xnear[2])]
-        a = [xnear, xparent]
-        self.E.remove_edge(a)  # remove and replace old the connection
+    def removewire(self,xnear):
+        xparent = self.Parent[hash3D(xnear)]
+        a = [xnear,xparent]
+        self.E.remove_edge(a) # remove and replace old the connection
+        #self.Parent.pop(hash3D(xnear), None)
+
+    def reached(self):
+        self.done = True
+        xn = near(self,self.env.goal)
+        c = [cost(self,x) for x in xn]
+        xncmin = xn[np.argmin(c)]
+        self.wireup(self.env.goal,xncmin)
+        self.V.append(self.env.goal)
+        self.Path,self.D = path(self)
 
     def run(self):
         self.V.append(self.env.start)
         ind = 0
         xnew = self.env.start
-        while ind < self.maxiter and getDist(xnew, self.env.goal) > 1:
-            # while ind < self.maxiter:
-            xrand = sampleFree(self)
-            xnearest = nearest(self, xrand)
-            xnew = steer(self, xnearest, xrand)
-            if not isCollide(self, xnearest, xnew):
-                Xnear = near(self, xnew)
-                self.V.append(xnew)  # add point
+        print('start rrt*... ')
+        while ind < self.maxiter:
+            xrand    = sampleFree(self)
+            xnearest = nearest(self,xrand)
+            xnew     = steer(self,xnearest,xrand)
+            if not isCollide(self,xnearest,xnew):
+                Xnear = near(self,xnew)
+                self.V.append(xnew) # add point
                 # visualization(self)
                 # minimal path and minimal cost
                 xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew)
@@ -70,16 +83,15 @@ class rrtstar():
                             self.wireup(xnear, xnew)
                 self.i += 1
             ind += 1
-            # when the goal is reached
-            if getDist(xnew, self.env.goal) <= 1:
-                self.wireup(self.env.goal, xnew)
-                self.Path, self.D = path(self)
+        # max sample reached
+        self.reached()
+        print('time used = ' + str(time.time()-starttime))
+        print('Total distance = '+str(self.D))
         visualization(self)
-        print('Total distance = ' + str(self.D))
-
+        
 
 if __name__ == '__main__':
     p = rrtstar()
     starttime = time.time()
     p.run()
-    print('time used = ' + str(time.time() - starttime))
+    

Sampling-based Planning/rrt_3D/utils3D.py

@@ -40,9 +40,10 @@ def sampleFree(initparams):
     if isinside(initparams, x):
         return sampleFree(initparams)
     else:
-        if i < 0.05:
-            return initparams.env.goal
-        else: return np.array(x)
+        #if i < 0.05:
+        #    return initparams.env.goal+0.01
+        #else: return np.array(x)
+        return np.array(x)
 
 
 def isinside(initparams, x):
@@ -88,8 +89,11 @@ def steer(initparams, x, y):
 
 
 def near(initparams, x):
-    # TODO: r = min{gamma*log(card(V)/card(V)1/d),eta}
-    r = initparams.stepsize
+    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]
@@ -103,31 +107,31 @@ def cost(initparams, x):
     '''here use the additive recursive cost function'''
     if all(x == initparams.env.start):
         return 0
-    xparent = initparams.Parent[str(x[0])][str(x[1])][str(x[2])]
+    xparent = initparams.Parent[hash3D(x)]
     return cost(initparams, xparent) + getDist(x, xparent)
 
 
 def path(initparams, Path=[], dist=0):
     x = initparams.env.goal
     while not all(x == initparams.env.start):
-        x2 = initparams.Parent[str(x[0])][str(x[1])][str(x[2])]
+        x2 = initparams.Parent[hash3D(x)]
         Path.append(np.array([x, x2]))
         dist += getDist(x, x2)
         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 = {}
 
-    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])
+        x, y = hash3D(edge[0]),hash3D(edge[1])
         if x in self.E:
             self.E[x].append(y)
         else:
@@ -135,12 +139,12 @@ class edgeset(object):
 
     def remove_edge(self,edge):
         x, y = edge[0],edge[1]
-        self.E[self.hash(x)].remove(self.hash(y))
+        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((self.dehash(v),self.dehash(n)))
+                edges.append((dehash(v),dehash(n)))
         return edges