'Update'

Sampling-based Planning/rrt_3D/dynamic_rrt3D.py

@@ -5,6 +5,7 @@ This is dynamic rrt code for 3D
 import numpy as np
 from numpy.matlib import repmat
 from collections import defaultdict
+import copy
 import time
 import matplotlib.pyplot as plt
 
@@ -13,108 +14,227 @@ 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, isinbound
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, cost, path, edgeset, isinbound, isinside
 from rrt_3D.rrt3D import rrt
+from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 
-class dynamic_rrt_3D(rrt):
+class dynamic_rrt_3D():
     
     def __init__(self):
-        # variables in rrt
         self.env = env()
-        self.Parent = {}
-        self.E = edgeset() # edgeset
-        self.V = [] # nodeset
-        self.i = 0
-        self.maxiter = 10000 # at least 2000 in this env
-        self.stepsize = 0.5
-        self.gamma = 500
-        self.eta = 2*self.stepsize
+        self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
+        self.qrobot = self.x0
+        self.current = tuple(self.env.start)
+        self.stepsize = 0.25
+        self.maxiter = 10000
+        self.GoalProb = 0.05 # probability biased to the goal
+        self.WayPointProb = 0.05 # probability falls back on to the way points
+
+        self.V = [] # vertices
+        self.Parent = {} # parent child relation
+        self.Edge = set() # edge relation (node, parent node) tuple
         self.Path = []
-        self.done = False
-        self.x0 = tuple(self.env.goal)
-        self.xt = tuple(self.env.start)
-        # additional variables
-        self.Flag = {}
-        self.xrobot = tuple(self.env.start)
-        
-        self.V.append(self.x0)
+        self.flag = {}# flag dictionary
         self.ind = 0
-        self.fig = plt.figure(figsize=(10, 8))
+        self.i = 0
 
-    def RegrowRRT(self, xrobot):
+#--------Dynamic RRT algorithm
+    def RegrowRRT(self):
         self.TrimRRT()
-        self.GrowRRT(xrobot = xrobot)
+        self.GrowRRT()
 
     def TrimRRT(self):
         S = []
         i = 1
-        for qi in self.V:
-            if qi == self.x0:
-                continue
+        print('trimming...')
+        while i < len(self.V):
+            qi = self.V[i]
             qp = self.Parent[qi]
-            if qp == self.x0:
-                continue
-            if self.Flag[qp] == 'Invalid':
-                self.Flag[qi] = 'Invalid'
-                self.E.remove_edge([qi,qp]) # REMOVE edge that parent is invalid
-            if self.Flag[qi] != 'Invalid':
+            if self.flag[qp] == 'Invalid':
+                self.flag[qi] = 'Invalid'
+            if self.flag[qi] != 'Invalid':
                 S.append(qi)
             i += 1
-        self.V, self.E = self.CreateTreeFromNodes(S)
+        self.CreateTreeFromNodes(S)
+        print('trimming complete...')
     
-    def InvalidateNodes(self, obstacle, mode):
-        E = self.FindAffectedEdges(obstacle, mode)
-        for e in E:
-            qe = self.ChildEndpointNode(e)
-            self.Flag[qe] = 'Invalid'
+    def InvalidateNodes(self, obstacle):
+        Edges = self.FindAffectedEdges(obstacle)
+        for edge in Edges:
+            qe = self.ChildEndpointNode(edge)
+            self.flag[qe] = 'Invalid'
+
+#--------Extend RRT algorithm-----
+    def initRRT(self):
+        self.V.append(self.x0)
+        self.flag[self.x0] = 'Valid'
+
+    def GrowRRT(self):
+        print('growing')
+        qnew = self.x0
+        tree = None
+        distance_threshold = self.stepsize
+        self.ind = 0
+        while self.ind <= self.maxiter:
+            qtarget = self.ChooseTarget()
+            qnearest = self.Nearest(tree, qtarget)
+            qnew, collide = self.Extend(qnearest, qtarget)
+            if not collide:
+                self.AddNode(qnearest, qnew)
+                if getDist(qnew, self.xt) < distance_threshold:
+                    self.AddNode(qnearest, self.xt)
+                    self.flag[self.xt] = 'Valid'
+                    break
+                self.i += 1
+            self.ind += 1
+            # self.visualization()
+        print('growing complete...')
+
+    def ChooseTarget(self):
+        # return the goal, or randomly choose a state in the waypoints based on probs
+        p = np.random.uniform()
+        if len(self.V) == 1:
+            i = 0
+        else:
+            i = np.random.randint(0, high = len(self.V) - 1)
+        if 0 < p < self.GoalProb:
+            return self.xt
+        elif self.GoalProb < p < self.GoalProb + self.WayPointProb:
+            return self.V[i]
+        elif self.GoalProb + self.WayPointProb < p < 1:
+            return tuple(self.RandomState())
+       
+    def RandomState(self):
+        # generate a random, obstacle free state
+        xrand = sampleFree(self, bias=0)
+        return xrand
+
+    def AddNode(self, nearest, extended):
+        self.V.append(extended)
+        self.Parent[extended] = nearest
+        self.Edge.add((extended, nearest))
+        self.flag[extended] = 'Valid'
+
+    def Nearest(self, tree, target):
+        # TODO use kdTree to speed up search
+        return nearest(self, target, isset=True)
 
+    def Extend(self, nearest, target):
+        extended, dist = steer(self, nearest, target, DIST = True)
+        collide, _ = isCollide(self, nearest, target, dist)
+        return extended, collide
+
+#--------Main function
     def Main(self):
-        qgoal = tuple(self.env.start)
-        qstart = tuple(self.env.goal)
+        # qstart = qgoal
+        self.x0 = tuple(self.env.goal)
+        # qgoal = qrobot
+        self.xt = tuple(self.env.start)
+        self.initRRT()
         self.GrowRRT()
-        self.done = True
-        # visualization(self)
-        self.done = False
-        # change the enviroment
-        new0,old0 = self.env.move_block(a=[0, 0, -2], s=0.5, block_to_move=1, mode='translation')
-        while qgoal != qstart:
-            qgoal = self.Parent[qgoal]
-            # TODO move to qgoal and check for new obs
-            xrobot = qstart
-            # TODO if any new obstacle are observed
-            self.InvalidateNodes(new0, mode = 'translation')
-            for xi in self.Path:
-                if self.Flag[tuple(xi[0])] == 'Invalid':
-                    self.RegrowRRT(tuple(self.env.start))
-        self.done = True
         self.Path, D = path(self)
-        visualization(self)
+        self.done = True
+        self.visualization() 
         plt.show()
-        
-    def GrowRRT(self, Reversed = True, xrobot = None):
-        # rrt.run()
-        self.run(Reversed = Reversed, xrobot = xrobot)
-
-    def CreateTreeFromNodes(self, S):
-        return S, self.E
-
-    def FindAffectedEdges(self, obstacle, mode):
-        # if the nodes appears inside of the new range an obstacle moved, 
-        # find the edges associated with this node. (the node is the parent)
-        nodes = np.array(self.V)
-        if mode == 'translation':
-            affected_nodes = nodes[isinbound(obstacle, nodes, isarray = True)]
-        elif mode == 'rotation':
-            affected_nodes = nodes[isinbound(obstacle, nodes, mode = 'obb', isarray = True)]
-        if len(affected_nodes) == 0:
-            return []
-        return self.E.get_edge(affected_nodes)
-
-    def ChildEndpointNode(self, e):
-        # if self.E.isEndNode(e[1]):
-        return e[1]
-        #else: 
-        #    return self.ChildEndpointNode(e[1])
+        t = 0
+        while True:
+            # move the block while the robot is moving
+            new, old = self.env.move_block(a=[0, 0, -0.2], mode='translation')
+            self.InvalidateNodes(new)
+            # if solution path contains invalid node
+            self.done = True
+            self.visualization()
+            plt.show()
+            invalid = self.PathisInvalid(self.Path)
+            if invalid:
+                self.done = False
+                self.RegrowRRT()
+                self.Path = []
+                self.Path, D = path(self)
+            
+            if t == 8:
+                break
+
+#--------Additional utility functions
+    def FindAffectedEdges(self, obstacle):
+        # scan the graph for the changed edges in the tree.
+        # return the end point and the affected
+        Affectededges = []
+        for e in self.Edge:
+            child, parent = e
+            collide, _ = isCollide(self, child, parent)
+            if collide:
+                Affectededges.append(e)
+        return Affectededges
+
+    def ChildEndpointNode(self, edge):
+        return edge[0]
+
+    def CreateTreeFromNodes(self, Nodes):
+        self.V = []
+        Parent = {}
+        edges = set()
+        for v in Nodes:
+            self.V.append(v)
+            Parent[v] = self.Parent[v]
+            edges.add((v, Parent[v]))
+        self.Parent = Parent
+        self.Edge = edges
+
+    def PathisInvalid(self, path):
+        for edge in path:
+            if self.flag[tuple(edge[0])] == 'Invalid' or self.flag[tuple(edge[1])] == 'Invalid':
+                return True
+
+    def path(self, Path=[], dist=0):
+        x = self.xt
+        while x != self.x0:
+            x2 = self.Parent[x]
+            Path.append(np.array([x, x2]))
+            dist += getDist(x, x2)
+            x = x2
+        return Path, dist
+            
+#--------Visualization specialized for dynamic RRT
+    def visualization(self):
+        if self.ind % 100 == 0 or self.done:
+            V = np.array(self.V)
+            Path = np.array(self.Path)
+            start = self.env.start
+            goal = self.env.goal
+            edges = []
+            for i in self.Parent:
+                edges.append([i,self.Parent[i]])
+            ax = plt.subplot(111, projection='3d')
+            # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
+            # ax.view_init(elev=0., azim=90.)
+            ax.view_init(elev=8., azim=120.)
+            ax.clear()
+            # drawing objects
+            draw_Spheres(ax, self.env.balls)
+            draw_block_list(ax, self.env.blocks)
+            if self.env.OBB is not None:
+                draw_obb(ax, self.env.OBB)
+            draw_block_list(ax, np.array([self.env.boundary]), alpha=0)
+            draw_line(ax, edges, visibility=0.75, color='g')
+            draw_line(ax, Path, color='r')
+            # if len(V) > 0:
+            #     ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', )
+            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')
+            # adjust the aspect ratio
+            xmin, xmax = self.env.boundary[0], self.env.boundary[3]
+            ymin, ymax = self.env.boundary[1], self.env.boundary[4]
+            zmin, zmax = self.env.boundary[2], self.env.boundary[5]
+            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.ylabel('y')
+            ax.set_axis_off()
+            plt.pause(0.0001)
+
+
 
 if __name__ == '__main__':
     rrt = dynamic_rrt_3D()

Sampling-based Planning/rrt_3D/env3D.py

@@ -80,6 +80,7 @@ def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+0
 
 class env():
     def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1):
+    # def __init__(self, xmin=-5, ymin=0, zmin=-5, xmax=10, ymax=5, zmax=10, resolution=1):  
         self.resolution = resolution
         self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) 
         self.blocks = getblocks()

Sampling-based Planning/rrt_3D/extend_rrt3D.py

@@ -0,0 +1,102 @@
+# Real-Time Randomized Path Planning for Robot Navigation∗
+"""
+This is rrt extend code for 3D
+@author: yue qi
+"""
+import numpy as np
+from numpy.matlib import repmat
+from collections import defaultdict
+import time
+import matplotlib.pyplot as plt
+
+import os
+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
+
+# here attempt to use a KD tree for the data structure implementation
+import scipy.spatial.kdtree as KDtree
+
+
+class extend_rrt(object):
+    def __init__(self):
+        self.env = env()
+        self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
+        self.current = tuple(self.env.start)
+        self.stepsize = 0.5
+        self.maxiter = 10000
+        self.GoalProb = 0.05 # probability biased to the goal
+        self.WayPointProb = 0.05 # probability falls back on to the way points
+
+        self.done = False
+        self.V = [] # vertices
+        self.Parent = {}
+        self.Path = []
+        self.ind = 0
+        self.i = 0
+
+    #--------- basic rrt algorithm----------
+    def RRTplan(self, env, initial, goal):
+        threshold = self.stepsize
+        nearest = initial # state structure
+        self.V.append(initial)
+        rrt_tree = initial # TODO KDtree structure
+        while self.ind <= self.maxiter:
+            target = self.ChooseTarget(goal)
+            nearest = self.Nearest(rrt_tree, target)
+            extended, collide = self.Extend(env, nearest, target)
+            if not collide:
+                self.AddNode(rrt_tree, nearest, extended)
+                if getDist(nearest, goal) <= threshold:
+                    self.AddNode(rrt_tree, nearest, self.xt)
+                    break
+                self.i += 1
+            self.ind += 1
+            visualization(self)
+            
+        # return rrt_tree
+        self.done = True
+        self.Path, D = path(self)
+        visualization(self)
+        plt.show()
+        
+
+    def Nearest(self, tree, target):
+        # TODO use kdTree to speed up search
+        return nearest(self, target, isset=True)
+
+    def Extend(self, env, nearest, target):
+        extended, dist = steer(self, nearest, target, DIST = True)
+        collide, _ = isCollide(self, nearest, target, dist)
+        return extended, collide
+
+    def AddNode(self, tree, nearest, extended):
+        # TODO use a kdtree
+        self.V.append(extended)
+        self.Parent[extended] = nearest
+
+    def RandomState(self):
+        # generate a random, obstacle free state
+        xrand = sampleFree(self, bias=0)
+        return xrand
+
+    #--------- insight to the rrt_extend
+    def ChooseTarget(self, state):
+        # return the goal, or randomly choose a state in the waypoints based on probs
+        p = np.random.uniform()
+        if len(self.V) == 1:
+            i = 0
+        else:
+            i = np.random.randint(0, high = len(self.V) - 1)
+        if 0 < p < self.GoalProb:
+            return self.xt
+        elif self.GoalProb < p < self.GoalProb + self.WayPointProb:
+            return self.V[i]
+        elif self.GoalProb + self.WayPointProb < p < 1:
+            return tuple(self.RandomState())
+        
+if __name__ == '__main__':
+    t = extend_rrt()
+    _ = t.RRTplan(t.env, t.x0, t.xt)

Sampling-based Planning/rrt_3D/plot_util3D.py

@@ -86,14 +86,24 @@ def draw_line(ax, SET, visibility=1, color=None):
 
 def visualization(initparams):
     if initparams.ind % 100 == 0 or initparams.done:
-        V = np.array(list(initparams.V))
-        E = initparams.E
+        #----------- list structure
+        # V = np.array(list(initparams.V))
+        # E = initparams.E
+        #----------- end
+        V = np.array(initparams.V)
+        # edges = initparams.E
         Path = np.array(initparams.Path)
         start = initparams.env.start
         goal = initparams.env.goal
-        edges = E.get_edge()
+        # edges = E.get_edge()
+        #----------- list structure
+        edges = []
+        for i in initparams.Parent:
+            edges.append([i,initparams.Parent[i]])
+        #----------- end
         # generate axis objects
         ax = plt.subplot(111, projection='3d')
+        
         # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
         # ax.view_init(elev=0., azim=90.)
         ax.view_init(elev=8., azim=120.)
@@ -105,10 +115,10 @@ def visualization(initparams):
         if initparams.env.OBB is not None:
             draw_obb(ax, initparams.env.OBB)
         draw_block_list(ax, np.array([initparams.env.boundary]), alpha=0)
-        draw_line(ax, edges, visibility=0.25)
+        draw_line(ax, edges, visibility=0.75, color='g')
         draw_line(ax, Path, color='r')
-        if len(V) > 0:
-            ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', )
+        # if len(V) > 0:
+        #     ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', )
         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')
         # adjust the aspect ratio
@@ -117,8 +127,10 @@ def visualization(initparams):
         zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5]
         dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
         ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
-        plt.xlabel('x')
-        plt.ylabel('y')
+        make_transparent(ax)
+        #plt.xlabel('x')
+        #plt.ylabel('y')
+        ax.set_axis_off()
         plt.pause(0.0001)
 
 
@@ -179,6 +191,15 @@ def make_get_proj(self, rx, ry, rz):
 
     return get_proj
 
+def make_transparent(ax):
+    # make the panes transparent
+    ax.xaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))
+    ax.yaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))
+    ax.zaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))
+    # make the grid lines transparent
+    ax.xaxis._axinfo["grid"]['color'] =  (1,1,1,0)
+    ax.yaxis._axinfo["grid"]['color'] =  (1,1,1,0)
+    ax.zaxis._axinfo["grid"]['color'] =  (1,1,1,0)
 
 if __name__ == '__main__':
     pass

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -14,7 +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
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path
 
 
 class rrt():
@@ -22,7 +22,7 @@ class rrt():
         self.env = env()
         self.Parent = {}
         self.V = []
-        self.E = edgeset()
+        # self.E = edgeset()
         self.i = 0
         self.maxiter = 10000
         self.stepsize = 0.5
@@ -30,27 +30,16 @@ class rrt():
         self.done = False
         self.x0 = tuple(self.env.start)
         self.xt = tuple(self.env.goal)
-        self.Flag = None
 
         
         self.ind = 0
         self.fig = plt.figure(figsize=(10, 8))
 
     def wireup(self, x, y):
-        self.E.add_edge([x, y])  # add edge
+        # self.E.add_edge([x, y])  # add edge
         self.Parent[x] = y
 
-    def run(self, Reversed = True, xrobot = None):
-        if Reversed:
-            if xrobot is None:
-                self.x0 = tuple(self.env.goal)
-                self.xt = tuple(self.env.start)
-            else:
-                self.x0 = tuple(self.env.goal)
-                self.xt = xrobot
-            xnew = self.env.goal
-        else:
-            xnew = self.env.start
+    def run(self):
         self.V.append(self.x0)
         while self.ind < self.maxiter:
             xrand = sampleFree(self)
@@ -59,25 +48,21 @@ class rrt():
             collide, _ = isCollide(self, xnearest, xnew)
             if not collide:
                 self.V.append(xnew)  # add point
-                if self.Flag is not None:
-                    self.Flag[xnew] = 'Valid'
                 self.wireup(xnew, xnearest)
 
                 if getDist(xnew, self.xt) <= self.stepsize:
                     self.wireup(self.xt, xnew)
                     self.Path, D = path(self)
                     print('Total distance = ' + str(D))
-                    if self.Flag is not None:
-                        self.Flag[self.xt] = 'Valid'
                     break
                 # visualization(self)
                 self.i += 1
             self.ind += 1
             # if the goal is really reached
             
-        # self.done = True
-        # visualization(self)
-        # plt.show()
+        self.done = True
+        visualization(self)
+        plt.show()
 
 
 if __name__ == '__main__':

Sampling-based Planning/rrt_3D/rrt_connect3D.py

@@ -0,0 +1,132 @@
+# rrt connect algorithm
+"""
+This is rrt connect implementation for 3D
+@author: yue qi
+"""
+import numpy as np
+from numpy.matlib import repmat
+from collections import defaultdict
+import time
+import matplotlib.pyplot as plt
+
+import os
+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
+
+class rrt_connect():
+    def __init__(self):
+        self.env = env()
+        self.Parent = {}
+        self.V = []
+        self.E = set()
+        self.i = 0
+        self.maxiter = 10000
+        self.stepsize = 0.5
+        self.Path = []
+        self.done = False
+        self.qinit = tuple(self.env.start)
+        self.qgoal = tuple(self.env.goal)
+        self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
+        self.qnew = None
+        
+        self.ind = 0
+        self.fig = plt.figure(figsize=(10, 8))
+
+
+#----------Normal RRT algorithm
+    def BUILD_RRT(self, qinit):
+        tree = Tree(qinit)
+        for k in range(self.maxiter):
+            qrand = self.RANDOM_CONFIG()
+            self.EXTEND(tree, qrand)
+        return tree
+
+    def EXTEND(self, tree, q):
+        qnear = tuple(self.NEAREST_NEIGHBOR(q, tree))
+        qnew, dist = steer(self, qnear, q)
+        self.qnew = qnew # store qnew outside
+        if self.NEW_CONFIG(q, qnear, qnew, dist=dist):
+            tree.add_vertex(qnew)
+            tree.add_edge(qnear, qnew)
+            if qnew == q:
+                return 'Reached'
+            else:
+                return 'Advanced'
+        return 'Trapped'
+
+    def NEAREST_NEIGHBOR(self, q, tree):
+        # find the nearest neighbor in the tree
+        V = np.array(tree.V)
+        if len(V) == 1:
+            return V[0]
+        xr = repmat(q, len(V), 1)
+        dists = np.linalg.norm(xr - V, axis=1)
+        return tuple(tree.V[np.argmin(dists)])
+
+    def RANDOM_CONFIG(self):
+        return tuple(sampleFree(self))
+
+    def NEW_CONFIG(self, q, qnear, qnew, dist = None):
+        # to check if the new configuration is valid or not by 
+        # making a move is used for steer
+        # check in bound
+        collide, _ = isCollide(self, qnear, qnew, dist = dist)
+        return not collide
+
+    #----------RRT connect algorithm
+    def CONNECT(self, Tree, q):
+        print('in connect')
+        while True:
+            S = self.EXTEND(Tree, q)
+            if S != 'Advanced':
+                break
+        return S
+
+    def RRT_CONNECT_PLANNER(self, qinit, qgoal):
+        Tree_A = Tree(qinit)
+        Tree_B = Tree(qgoal)
+        for k in range(self.maxiter):
+            print(k)
+            qrand = self.RANDOM_CONFIG()
+            if self.EXTEND(Tree_A, qrand) != 'Trapped':
+                print('trapped')
+                qnew = self.qnew # get qnew from outside
+                if self.CONNECT(Tree_B, qnew) == 'Reached':
+                    print('reached')
+                    # return self.PATH(Tree_A, Tree_B)
+                    return
+            Tree_A, Tree_B = self.SWAP(Tree_A, Tree_B)
+        print('Failure')
+        return 'Failure'
+
+    # def PATH(self, tree_a, tree_b):
+
+
+    def SWAP(self, tree_a, tree_b):
+        tree_a, tree_b = tree_b, tree_a
+        return tree_a, tree_b
+
+class Tree():
+    def __init__(self, node):
+        self.V = []
+        self.Parent = {}
+        self.V.append(node)
+        self.Parent[node] = None
+
+    def add_vertex(self, node):
+        if node not in self.V:
+            self.V.append(node)
+        
+    def add_edge(self, parent, child):
+        # here edge is defined a tuple of (parent, child) (qnear, qnew)
+        self.Parent[child] = parent
+
+if __name__ == '__main__':
+    p = rrt_connect()
+    starttime = time.time()
+    p.RRT_CONNECT_PLANNER(p.qinit, p.qgoal)
+    print('time used = ' + str(time.time() - starttime))

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -13,20 +13,23 @@ 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
 
 
 class rrtstar():
     def __init__(self):
         self.env = env()
+
         self.Parent = {}
-        self.E = edgeset()
         self.V = []
+        # self.E = edgeset()
+        self.COST = {}
+
         self.i = 0
-        self.maxiter = 5000 # at least 2000 in this env
+        self.maxiter = 12000 # at least 2000 in this env
         self.stepsize = 0.5
         self.gamma = 500
-        self.eta = 2*self.stepsize
+        self.eta = self.stepsize
         self.Path = []
         self.done = False
         self.x0 = tuple(self.env.start)
@@ -35,13 +38,13 @@ class rrtstar():
         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([x,y]) # add edge
         self.Parent[x] = y
 
     def removewire(self,xnear):
         xparent = self.Parent[xnear]
         a = [xnear,xparent]
-        self.E.remove_edge(a) # remove and replace old the connection
+        # self.E.remove_edge(a) # remove and replace old the connection
 
     def reached(self):
         self.done = True
@@ -65,24 +68,26 @@ class rrtstar():
             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
+                Collide = []
                 for xnear in Xnear:
                     xnear = tuple(xnear)
                     c1 = cost(self, xnear) + getDist(xnew, xnear)
                     collide, _ = isCollide(self, xnew, xnear)
+                    Collide.append(collide)
                     if not collide and c1 < cmin:
                         xmin, cmin = xnear, c1
                 self.wireup(xnew, xmin)
                 # rewire
-                for xnear in Xnear:
-                    xnear = tuple(xnear)
+                for i in range(len(Xnear)):
+                    collide = Collide[i]
+                    xnear = tuple(Xnear[i])
                     c2 = cost(self, xnew) + getDist(xnew, xnear)
-                    collide, _ = isCollide(self, xnew, xnear)
                     if not collide and c2 < cost(self, xnear):
-                        self.removewire(xnear)
+                        # self.removewire(xnear)
                         self.wireup(xnear, xnew)
                 self.i += 1
             self.ind += 1

Sampling-based Planning/rrt_3D/utils3D.py

@@ -1,6 +1,7 @@
 import numpy as np
 from numpy.matlib import repmat
 import pyrr as pyrr
+from collections import deque
 
 import os
 import sys
@@ -166,7 +167,7 @@ def isCollide(initparams, x, child, dist=None):
     return False, dist
 
 # ---------------------- leaf node extending algorithms
-def nearest(initparams, x):
+def nearest(initparams, x, isset=False):
     V = np.array(initparams.V)
     if initparams.i == 0:
         return initparams.V[0]
@@ -175,28 +176,33 @@ def nearest(initparams, x):
     return tuple(initparams.V[np.argmin(dists)])
 
 def near(initparams, x):
-    x = np.array(x)
+    # x = np.array(x)
     V = np.array(initparams.V)
+    if initparams.i == 0:
+        return [initparams.V[0]]
     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 ** (1/3)), eta)
     if initparams.done: 
         r = 1
-    if initparams.i == 0:
-        return [initparams.V[0]]
     xr = repmat(x, len(V), 1)
     inside = np.linalg.norm(xr - V, axis=1) < r
     nearpoints = V[inside]
     return np.array(nearpoints)
 
-def steer(initparams, x, y):
+def steer(initparams, x, y, DIST=False):
+    # steer from x to y
+    if np.equal(x, y).all():
+        return x, 0.0
     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
+    if DIST:
+        return xnew, dist
+    return xnew, dist
 
 def cost(initparams, x):
     '''here use the additive recursive cost function'''
@@ -204,6 +210,11 @@ def cost(initparams, x):
         return 0
     return cost(initparams, initparams.Parent[x]) + getDist(x, initparams.Parent[x])
 
+def cost_from_set(initparams, x):
+    '''here use a incremental cost set function'''
+    if x == initparams.x0:
+        return 0
+    return initparams.COST[initparams.Parent[x]] + getDist(x, initparams.Parent[x])
 
 def path(initparams, Path=[], dist=0):
     x = initparams.xt
@@ -246,22 +257,156 @@ class edgeset(object):
     def isEndNode(self, node):
         return node not in self.E
 
-
+#------------------------ use a linked list to express the tree 
 class Node:
     def __init__(self, data):
-        self.data = data
-        self.sibling = None
-        self.child = None
-
-class Tree:
-    def __init__(self, start):
-        self.root = Node(start)
-        self.ind = 0
-        self.index = {start:self.ind}
+        self.pos = data
+        self.Parent = None
+        self.child = set()
+
+def tree_add_edge(node_in_tree, x):
+    # add an edge at the specified parent
+    node_to_add = Node(x)
+    # node_in_tree = tree_bfs(head, xparent)
+    node_in_tree.child.add(node_to_add)
+    node_to_add.Parent = node_in_tree
+    return node_to_add
+
+def tree_bfs(head, x):
+    # searches x in order of bfs
+    node = head
+    Q = []
+    Q.append(node)
+    while Q:
+        curr = Q.pop()
+        if curr.pos == x:
+            return curr
+        for child_node in curr.child:
+            Q.append(child_node)
+
+def tree_nearest(head, x):
+    # find the node nearest to x
+    D = np.inf
+    min_node = None
+
+    Q = []
+    Q.append(head)
+    while Q:
+        curr = Q.pop()
+        dist = getDist(curr.pos, x)
+        # record the current best
+        if dist < D:
+            D, min_node = dist, curr
+        # bfs
+        for child_node in curr.child:
+            Q.append(child_node)
+    return min_node
+
+def tree_steer(initparams, node, x):
+    # steer from node to x
+    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])
+    return xnew
 
-    def add_edge(self, edge):
-        # y exists in the tree while x does not
-        x, y = edge[0], edge[1]
+def tree_print(head):
+    Q = []
+    Q.append(head)
+    verts = []
+    edge = []
+    while Q:
+        curr = Q.pop()
+       # print(curr.pos)
+        verts.append(curr.pos)
+        if curr.Parent == None:
+            pass
+        else:
+            edge.append([curr.pos, curr.Parent.pos])
+        for child in curr.child:
+            Q.append(child)
+    return verts, edge
+
+def tree_path(initparams, end_node):
+    path = []
+    curr = end_node
+    while curr.pos != initparams.x0:
+        path.append([curr.pos, curr.Parent.pos])
+        curr = curr.Parent
+    return path
+
+
+#---------------KD tree, used for nearest neighbor search
+class kdTree:
+    def __init__(self):
+        pass
+
+    def R1_dist(self, q, p):
+        return abs(q-p)
+
+    def S1_dist(self, q, p):
+        return min(abs(q-p), 1- abs(q-p))
+
+    def P3_dist(self, q, p):
+        # cubes with antipodal points
+        q1, q2, q3 = q
+        p1, p2, p3 = p
+        d1 = np.sqrt((q1-p1)**2 + (q2-p2)**2 + (q3-p3)**2)
+        d2 = np.sqrt((1-abs(q1-p1))**2 + (1-abs(q2-p2))**2 + (1-abs(q3-p3))**2)
+        d3 = np.sqrt((-q1-p1)**2 + (-q2-p2)**2 + (q3+1-p3)**2)
+        d4 = np.sqrt((-q1-p1)**2 + (-q2-p2)**2 + (q3-1-p3)**2)
+        d5 = np.sqrt((-q1-p1)**2 + (q2+1-p2)**2 + (-q3-p3)**2)
+        d6 = np.sqrt((-q1-p1)**2 + (q2-1-p2)**2 + (-q3-p3)**2)
+        d7 = np.sqrt((q1+1-p1)**2 + (-q2-p2)**2 + (-q3-p3)**2)
+        d8 = np.sqrt((q1-1-p1)**2 + (-q2-p2)**2 + (-q3-p3)**2)
+        return min(d1,d2,d3,d4,d5,d6,d7,d8)
+
+
+
+if __name__ == '__main__':
+    from rrt_3D.env3D import env
+    import time
+    import matplotlib.pyplot as plt
+    class rrt_demo:
+        def __init__(self):
+            self.env = env()
+            self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
+            self.stepsize = 0.5
+            self.maxiter = 10000
+            self.ind, self.i = 0, 0
+            self.done = False
+            self.Path = []
+            self.V = []
+
+            self.head = Node(self.x0)
+        
+        def run(self):
+            while self.ind < self.maxiter:
+                xrand = sampleFree(self) # O(1)
+                nearest_node = tree_nearest(self.head, xrand) # O(N)
+                xnew = tree_steer(self, nearest_node, xrand) # O(1)
+                collide, _ = isCollide(self, nearest_node.pos, xnew) # O(num obs)
+                if not collide:
+                    new_node = tree_add_edge(nearest_node, xnew) # O(1)
+                    # if the path is found
+                    if getDist(xnew, self.xt) <= self.stepsize:
+                        end_node = tree_add_edge(new_node, self.xt)
+                        self.Path = tree_path(self, end_node)
+                        break
+                    self.i += 1
+                self.ind += 1
+            
+            self.done = True
+            self.V, self.E = tree_print(self.head)
+            print(self.E)
+            visualization(self)
+            plt.show()
+            
+
+    
+    A = rrt_demo()
+    st = time.time()
+    A.run()
+    print(time.time() - st)