yue qi

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -5,7 +5,7 @@ This is rrt star code for 3D
 """
 import numpy as np
 from numpy.matlib import repmat
-from rrt_3D.env3D import env
+from env3D import env
 from collections import defaultdict
 import pyrr as pyrr
 from utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path
@@ -27,11 +27,6 @@ class rrtstar():
         self.E.append([x,y]) # add edge
         self.Parent[str(x[0])][str(x[1])][str(x[2])] = y
 
-    def removewire(self,xnear):
-        xparent = self.Parent[str(xnear[0])][str(xnear[1])][str(xnear[2])]
-        a = np.array([xnear,xparent])
-        self.E = [xx for xx in self.E if not (xx==a).all()] # remove and replace old the connection
-
     def run(self):
         self.V.append(self.env.start)
         ind = 0

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -5,10 +5,10 @@ This is rrt star code for 3D
 """
 import numpy as np
 from numpy.matlib import repmat
-from rrt_3D.env3D import env
+from 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
+from utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path
 import time
 
 
@@ -66,6 +66,7 @@ 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,D = path(self)