diff --git a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc index 9aafad5..15349fd 100644 Binary files a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc and b/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc index 42deba7..90cfeab 100644 Binary files a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc and b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_3D/rrt3D.py b/Sampling-based Planning/rrt_3D/rrt3D.py index 0700580..bfd5bc8 100644 --- a/Sampling-based Planning/rrt_3D/rrt3D.py +++ b/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 diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py index bcd6418..b00d5c7 100644 --- a/Sampling-based Planning/rrt_3D/rrtstar3D.py +++ b/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)