LPA*

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -45,7 +45,7 @@ class rrtstar():
             if not isCollide(self, xnearest, xnew):
                 self.V.append(xnew)  # add point
                 self.wireup(xnew, xnearest)
-                # visualization(self)
+                visualization(self)
                 self.i += 1
             self.ind += 1
             if getDist(xnew, self.env.goal) <= 1:

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -26,7 +26,7 @@ class rrtstar():
         self.maxiter = 10000 # at least 4000 in this env
         self.stepsize = 0.5
         self.gamma = 500
-        self.eta = 1.1*self.stepsize
+        self.eta = 2*self.stepsize
         self.Path = []
         self.done = False
 
@@ -61,7 +61,7 @@ class rrtstar():
             if not isCollide(self,xnearest,xnew):
                 Xnear = near(self,xnew)
                 self.V.append(xnew) # add point
-                # visualization(self)
+                visualization(self)
                 # minimal path and minimal cost
                 xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew)
                 # connecting along minimal cost path

Sampling-based Planning/rrt_3D/utils3D.py

@@ -40,9 +40,9 @@ def sampleFree(initparams):
     if isinside(initparams, x):
         return sampleFree(initparams)
     else:
-        #if i < 0.05:
-        #    return initparams.env.goal+0.01
-        #else: return np.array(x)
+        if i < 0.05:
+           return initparams.env.goal+0.01
+        else: return np.array(x)
         return np.array(x)