'env'

Sampling_based_Planning/rrt_3D/BIT_star3D.py

@@ -72,6 +72,12 @@ class BIT_star:
         # denote if the path is found 
         self.done = False
         self.Path = []
+        
+        # for drawing the ellipse
+        self.C = np.zeros([3,3])
+        self.L = np.zeros([3,3])
+        self.xcenter = np.zeros(3)
+        self.show_ellipse = show_ellipse
 
     def run(self):
         self.V = {self.xstart} # node expanded
@@ -97,7 +103,7 @@ class BIT_star:
                 self.QV = {v for v in self.V}
                 # setting the radius 
                 if self.done:
-                    self.r = 1 # sometimes the original radius criteria makes the radius too small to improve existing tree
+                    self.r = 2 # sometimes the original radius criteria makes the radius too small to improve existing tree
                     num_resample += 1
                 else:
                     self.r = self.radius(len(self.V) + len(self.Xsamples)) # radius determined with the sample size and dimension of conf space
@@ -333,6 +339,7 @@ class BIT_star:
             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
+            ax.dist = 5
             set_axes_equal(ax)
             make_transparent(ax)
             #plt.xlabel('s')
@@ -342,7 +349,5 @@ class BIT_star:
 
 
 if __name__ == '__main__':
-    Newprocess = BIT_star()
-    Newprocess.run()
-    # Xsamples = Newprocess.Sample(1000, 140)
-    # print(len(Xsamples))
+    Newprocess = BIT_star(show_ellipse=False)
+    Newprocess.run()

Sampling_based_Planning/rrt_3D/env3D.py

@@ -22,7 +22,7 @@ def R_matrix(z_angle,y_angle,x_angle):
 def getblocks():
     # AABBs
     block = [[4.00e+00, 1.20e+01, 0.00e+00, 5.00e+00, 2.00e+01, 5.00e+00],
-             [5.00e+00, 1.20e+01, 0.00e+00, 1.00e+01, 1.30e+01, 5.00e+00],
+             [5.5e+00, 1.20e+01, 0.00e+00, 1.00e+01, 1.30e+01, 5.00e+00],
              [1.00e+01, 1.20e+01, 0.00e+00, 1.40e+01, 1.30e+01, 5.00e+00],
              [1.00e+01, 9.00e+00, 0.00e+00, 2.00e+01, 1.00e+01, 5.00e+00],
              [9.00e+00, 6.00e+00, 0.00e+00, 1.00e+01, 1.00e+01, 5.00e+00]]

Sampling_based_Planning/rrt_3D/informed_rrt_star3D.py

@@ -47,11 +47,11 @@ class IRRT:
         self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
         self.Parent = {}
         self.Path = []
-        self.N = 5000 # used for determining how many batches needed
+        self.N = 10000 # used for determining how many batches needed
         self.ind = 0
         self.i = 0
         # rrt* near and other utils
-        self.stepsize = 0.5
+        self.stepsize = 1
         self.gamma = 500
         self.eta = self.stepsize
         self.rgoal = self.stepsize
@@ -186,7 +186,7 @@ class IRRT:
         return getDist(x, y)
 
     def visualization(self):
-        if self.ind % 100 == 0:
+        if self.ind % 500 == 0:
             V = np.array(self.V)
             edges = list(map(list, self.E))
             Path = np.array(self.Path)
@@ -221,6 +221,7 @@ class IRRT:
             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
+            ax.dist = 5
             set_axes_equal(ax)
             make_transparent(ax)
             #plt.xlabel('s')

Sampling_based_Planning/rrt_3D/plot_util3D.py

@@ -122,6 +122,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')
         # adjust the aspect ratio
+        ax.dist = 5
         set_axes_equal(ax)
         make_transparent(ax)
         #plt.xlabel('s')