'rrt'

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -21,12 +21,13 @@ from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near,
 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,7 +40,7 @@ class rrtstar():
         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)
@@ -47,13 +48,17 @@ class rrtstar():
             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()

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 @@ class edgeset(object):
         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]