'rrt'

zhenya-lei · Jul 26, 2020 · 89a04e5 · 89a04e5
1 parent f0753b4
commit 89a04e5
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 13 deletions.
diff --git a/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc
diff --git a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc
diff --git a/Sampling-based Planning/rrt_3D/rrt3D.py b/Sampling-based Planning/rrt_3D/rrt3D.py
@@ -21,12 +21,13 @@
 class rrtstar():
     def __init__(self):
         self.env = env()
-        self.Parent = defaultdict(lambda: defaultdict(dict))
+        # self.Parent = defaultdict(lambda: defaultdict(dict))
+        self.Parent = {}
         self.V = []
         self.E = edgeset()
         self.i = 0
         self.maxiter = 10000
-        self.stepsize = 1.0
+        self.stepsize = 0.5
         self.Path = []
         self.done = False
 
@@ -39,21 +40,25 @@ def run(self):
         self.ind = 0
         self.fig = plt.figure(figsize=(10, 8))
         xnew = self.env.start
-        while self.ind < self.maxiter and getDist(xnew, self.env.goal) > 1:
+        while self.ind < self.maxiter and getDist(xnew, self.env.goal) > self.stepsize:
             xrand = sampleFree(self)
             xnearest = nearest(self, xrand)
             xnew = steer(self, xnearest, xrand)
             collide, _ = isCollide(self, xnearest, xnew)
             if not collide:
                 self.V.append(xnew)  # add point
                 self.wireup(xnew, xnearest)
+
+                if getDist(xnew, self.env.goal) <= self.stepsize:
+                    goal = tuple(self.env.goal)
+                    self.wireup(goal, xnew)
+                    self.Path, D = path(self)
+                    print('Total distance = ' + str(D))
                 # visualization(self)
                 self.i += 1
             self.ind += 1
-            if getDist(xnew, self.env.goal) <= 1:
-                self.wireup(self.env.goal, xnew)
-                self.Path, D = path(self)
-                print('Total distance = ' + str(D))
+            # if the goal is really reached
+
         self.done = True
         visualization(self)
         plt.show()

diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py
@@ -169,9 +169,12 @@ def nearest(initparams, x):
 
 
 def steer(initparams, x, y):
-    direc = (y - x) / np.linalg.norm(y - x)
-    xnew = x + initparams.stepsize * direc
-    return tuple(xnew)
+    dist, step = getDist(y, x), initparams.stepsize
+    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)
+    # xnew = x + initparams.stepsize * direc
+    return xnew
 
 
 def near(initparams, x):
@@ -199,8 +202,8 @@ def cost(initparams, x):
 
 
 def path(initparams, Path=[], dist=0):
-    x = initparams.env.goal
-    while not all(x == initparams.env.start):
+    x = tuple(initparams.env.goal)
+    while x != tuple(initparams.env.start):
         x2 = initparams.Parent[x]
         Path.append(np.array([x, x2]))
         dist += getDist(x, x2)
@@ -225,7 +228,8 @@ def add_edge(self, edge):
         if x in self.E:
             self.E[x].add(y)
         else:
-            self.E[x] = set(y)
+            self.E[x] = set()
+            self.E[x].add(y)
 
     def remove_edge(self, edge):
         x, y = edge[0], edge[1]