changed path

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -5,12 +5,17 @@ This is rrt star code for 3D
 """
 import numpy as np
 from numpy.matlib import repmat
-from rrt_3D.env3D import env
 from collections import defaultdict
-import pyrr as pyrr
-from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
 import time
 
+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 rrtstar():
     def __init__(self):

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -5,12 +5,16 @@ This is rrt star code for 3D
 """
 import numpy as np
 from numpy.matlib import repmat
-from rrt_3D.env3D import env
 from collections import defaultdict
-import pyrr as pyrr
-from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
 import time
 
+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 rrtstar():
     def __init__(self):
@@ -37,6 +41,7 @@ class rrtstar():
         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)
@@ -69,9 +74,9 @@ class rrtstar():
             # when the goal is reached
             if getDist(xnew,self.env.goal) <= 1:
                 self.wireup(self.env.goal,xnew)
-                self.Path,D = path(self)
-                print('Total distance = '+str(D))
+                self.Path,self.D = path(self)
         visualization(self)
+        print('Total distance = '+str(self.D))
 
 if __name__ == '__main__':
     p = rrtstar()