'dynamic_rrt'

.idea/PathPlanning.iml

@@ -2,11 +2,10 @@
 <module type="PYTHON_MODULE" version="4">
   <component name="NewModuleRootManager">
     <content url="file://$MODULE_DIR$" />
-    <orderEntry type="inheritedJdk" />
+    <orderEntry type="jdk" jdkName="Python 3.7 (base)" jdkType="Python SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
   <component name="TestRunnerService">
-    <option name="projectConfiguration" value="pytest" />
     <option name="PROJECT_TEST_RUNNER" value="pytest" />
   </component>
 </module>

.idea/misc.xml

@@ -1,4 +1,4 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7 (base)" project-jdk-type="Python SDK" />
 </project>

Sampling_based_Planning/rrt_2D/rrt.py

@@ -41,6 +41,7 @@ class Rrt:
         self.obs_boundary = self.env.obs_boundary
 
     def planning(self):
+        print("z")
         for i in range(self.iter_max):
             node_rand = self.generate_random_node(self.goal_sample_rate)
             node_near = self.nearest_neighbor(self.vertex, node_rand)
@@ -102,7 +103,7 @@ def main():
     x_start = (2, 2)  # Starting node
     x_goal = (49, 24)  # Goal node
 
-    rrt = Rrt(x_start, x_goal, 0.5, 0.00, 10000)
+    rrt = Rrt(x_start, x_goal, 0.5, 0.05, 10000)
     path = rrt.planning()
 
     if path:

Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py

@@ -12,14 +12,16 @@ import matplotlib.pyplot as plt
 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.env3D import env
-from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, cost, path, edgeset, isinbound, isinside
+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():
-    
+
+class dynamic_rrt_3D:
+
     def __init__(self):
         self.env = env()
         self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
@@ -27,18 +29,20 @@ class dynamic_rrt_3D():
         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.GoalProb = 0.05  # probability biased to the goal
+        self.WayPointProb = 0.02  # probability falls back on to the way points
+        self.done = False
+        self.invalid = False
+
+        self.V = []  # vertices
+        self.Parent = {}  # parent child relation
+        self.Edge = set()  # edge relation (node, parent node) tuple
         self.Path = []
-        self.flag = {}# flag dictionary
+        self.flag = {}  # flag dictionary
         self.ind = 0
         self.i = 0
 
-#--------Dynamic RRT algorithm
+    # --------Dynamic RRT algorithm
     def RegrowRRT(self):
         self.TrimRRT()
         self.GrowRRT()
@@ -57,14 +61,14 @@ class dynamic_rrt_3D():
             i += 1
         self.CreateTreeFromNodes(S)
         print('trimming complete...')
-    
+
     def InvalidateNodes(self, obstacle):
         Edges = self.FindAffectedEdges(obstacle)
         for edge in Edges:
             qe = self.ChildEndpointNode(edge)
             self.flag[qe] = 'Invalid'
 
-#--------Extend RRT algorithm-----
+    # --------Extend RRT algorithm-----
     def initRRT(self):
         self.V.append(self.x0)
         self.flag[self.x0] = 'Valid'
@@ -72,12 +76,11 @@ class dynamic_rrt_3D():
     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)
+            qnearest = self.Nearest(qtarget)
             qnew, collide = self.Extend(qnearest, qtarget)
             if not collide:
                 self.AddNode(qnearest, qnew)
@@ -96,14 +99,14 @@ class dynamic_rrt_3D():
         if len(self.V) == 1:
             i = 0
         else:
-            i = np.random.randint(0, high = len(self.V) - 1)
+            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)
@@ -115,16 +118,16 @@ class dynamic_rrt_3D():
         self.Edge.add((extended, nearest))
         self.flag[extended] = 'Valid'
 
-    def Nearest(self, tree, target):
+    def Nearest(self, 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)
+        extended, dist = steer(self, nearest, target, DIST=True)
         collide, _ = isCollide(self, nearest, target, dist)
         return extended, collide
 
-#--------Main function
+    # --------Main function
     def Main(self):
         # qstart = qgoal
         self.x0 = tuple(self.env.goal)
@@ -132,33 +135,36 @@ class dynamic_rrt_3D():
         self.xt = tuple(self.env.start)
         self.initRRT()
         self.GrowRRT()
-        self.Path, D = path(self)
+        self.Path, D = self.path()
         self.done = True
-        self.visualization() 
-        plt.show()
+        self.visualization()
         t = 0
         while True:
             # move the block while the robot is moving
             new, _ = self.env.move_block(a=[0, 0, -0.2], mode='translation')
             self.InvalidateNodes(new)
+            self.TrimRRT()
             # if solution path contains invalid node
-            self.done = True
             self.visualization()
-            plt.show()
-            invalid = self.PathisInvalid(self.Path)
-            if invalid:
+            self.invalid = self.PathisInvalid(self.Path)
+            if self.invalid:
                 self.done = False
                 self.RegrowRRT()
                 self.Path = []
-                self.Path, D = path(self)
-            
+                self.Path, D = self.path()
+                self.done = True
+                self.visualization()
             if t == 8:
                 break
+            t += 1
+        self.visualization()
+        plt.show()
 
-#--------Additional utility functions
+    # --------Additional utility functions
     def FindAffectedEdges(self, obstacle):
         # scan the graph for the changed edges in the tree.
         # return the end point and the affected
+        print('finding affected edges')
         Affectededges = []
         for e in self.Edge:
             child, parent = e
@@ -171,44 +177,48 @@ class dynamic_rrt_3D():
         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
+        print('creating tree')
+        # self.Parent = {node: self.Parent[node] for node in Nodes}
+        self.V = [node for node in Nodes]
+        self.Edge = {(node, self.Parent[node]) for node in Nodes}
+        # if self.invalid:
+        #     del self.Parent[self.xt]
 
     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):
+    def path(self, dist=0):
+        Path=[]
         x = self.xt
+        i = 0
         while x != self.x0:
             x2 = self.Parent[x]
             Path.append(np.array([x, x2]))
             dist += getDist(x, x2)
             x = x2
+            if i > 10000:
+                print('Path is not found')
+                return 
+            i+= 1
         return Path, dist
-            
-#--------Visualization specialized for dynamic RRT
+
+    # --------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]])
+            # edges = []
+            # for i in self.Parent:
+            #     edges.append([i, self.Parent[i]])
+            edges = np.array([list(i) for i in self.Edge])
             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.view_init(elev=0., azim=90.)
             ax.clear()
             # drawing objects
             draw_Spheres(ax, self.env.balls)
@@ -229,13 +239,12 @@ class dynamic_rrt_3D():
             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')
+            # plt.xlabel('x')
+            # plt.ylabel('y')
             ax.set_axis_off()
             plt.pause(0.0001)
 
 
-
 if __name__ == '__main__':
     rrt = dynamic_rrt_3D()
     rrt.Main()

Sampling_based_Planning/rrt_3D/plot_util3D.py

@@ -106,7 +106,7 @@ def visualization(initparams):
         
         # 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.view_init(elev=8., azim=90.)
         # ax.view_init(elev=-8., azim=180)
         ax.clear()
         # drawing objects

Sampling_based_Planning/rrt_3D/rrt_connect3D.py

@@ -18,6 +18,23 @@ from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
 from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 
+
+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
+
+
 class rrt_connect():
     def __init__(self):
         self.env = env()
@@ -33,6 +50,7 @@ class rrt_connect():
         self.qgoal = tuple(self.env.goal)
         self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
         self.qnew = None
+        self.done = False
         
         self.ind = 0
         self.fig = plt.figure(figsize=(10, 8))
@@ -78,7 +96,7 @@ class rrt_connect():
         collide, _ = isCollide(self, qnear, qnew, dist = dist)
         return not collide
 
-    #----------RRT connect algorithm
+#----------RRT connect algorithm
     def CONNECT(self, Tree, q):
         print('in connect')
         while True:
@@ -97,13 +115,14 @@ class rrt_connect():
                 qnew = self.qnew # get qnew from outside
                 if self.CONNECT(Tree_B, qnew) == 'Reached':
                     print('reached')
-                    # return self.PATH(Tree_A, Tree_B)
+                    self.done = True
+                    self.Path = self.PATH(Tree_A, Tree_B)
+                    self.visualization(Tree_A, Tree_B, k)
+                    plt.show()
                     return
-                else: 
-                    print('not reached')
+                    # return
             Tree_A, Tree_B = self.SWAP(Tree_A, Tree_B)
             self.visualization(Tree_A, Tree_B, k)
-        print('Failure')
         return 'Failure'
 
     # def PATH(self, tree_a, tree_b):
@@ -111,10 +130,29 @@ class rrt_connect():
         tree_a, tree_b = tree_b, tree_a
         return tree_a, tree_b
 
+    def PATH(self, tree_a, tree_b):
+        qnew = self.qnew
+        patha = []
+        pathb = []
+        while True:
+            patha.append((tree_a.Parent[qnew], qnew))
+            qnew = tree_a.Parent[qnew]
+            if qnew == self.qinit or qnew == self.qgoal:
+                break
+        qnew = self.qnew
+        while True:
+            pathb.append((tree_b.Parent[qnew], qnew))
+            qnew = tree_b.Parent[qnew]
+            if qnew == self.qinit or qnew == self.qgoal:
+                break
+        return patha + pathb
+
+#----------RRT connect algorithm        
     def visualization(self, tree_a, tree_b, index):
-        if (index % 10 == 0 and index != 0) or self.done:
+        if (index % 20 == 0 and index != 0) or self.done:
             # a_V = np.array(tree_a.V)
             # b_V = np.array(tree_b.V)
+            Path = self.Path
             start = self.env.start
             goal = self.env.goal
             a_edges, b_edges = [], []
@@ -132,32 +170,19 @@ class rrt_connect():
             draw_block_list(ax, np.array([self.env.boundary]), alpha=0)
             draw_line(ax, a_edges, visibility=0.75, color='g')
             draw_line(ax, b_edges, visibility=0.75, color='y')
-            # draw_line(ax, Path, color='r')
+            draw_line(ax, Path, color='r')
             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')
             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
+            dx, dy, _ = xmax - xmin, ymax - ymin, zmax - zmin
             ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
             make_transparent(ax)
             ax.set_axis_off()
             plt.pause(0.0001)
 
-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()

Sampling_based_Planning/rrt_3D/utils3D.py

@@ -196,6 +196,7 @@ def steer(initparams, x, y, DIST=False):
     if np.equal(x, y).all():
         return x, 0.0
     dist, step = getDist(y, x), initparams.stepsize
+    step = min(dist, step)
     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)