reformat

Sampling-based Planning/.idea/Sampling-based Planning.iml

@@ -2,7 +2,7 @@
 <module type="PYTHON_MODULE" version="4">
   <component name="NewModuleRootManager">
     <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="Python 3.7" jdkType="Python SDK" />
+    <orderEntry type="jdk" jdkName="Python 3.7 (Search-based Planning)" jdkType="Python SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
 </module>

Sampling-based Planning/.idea/misc.xml

@@ -1,4 +1,4 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7 (Search-based Planning)" project-jdk-type="Python SDK" />
 </project>

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -1,4 +1,3 @@
-
 """
 This is rrt star code for 3D
 @author: yue qi
@@ -10,13 +9,13 @@ import time
 
 import os
 import sys
-sys.path.append(os.path.dirname(os.path.abspath(__file__))+"/../../Sampling-based Planning/")
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
 
 from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
 
 
-
 class rrtstar():
     def __init__(self):
         self.env = env()
@@ -28,32 +27,33 @@ class rrtstar():
         self.stepsize = 0.5
         self.Path = []
 
-    def wireup(self,x,y):
-        self.E.add_edge([x,y]) # add edge
+    def wireup(self, x, y):
+        self.E.add_edge([x, y])  # add edge
         self.Parent[str(x[0])][str(x[1])][str(x[2])] = y
 
     def run(self):
         self.V.append(self.env.start)
         ind = 0
         xnew = self.env.start
-        while ind < self.maxiter and getDist(xnew,self.env.goal) > 1:
-            xrand    = sampleFree(self)
-            xnearest = nearest(self,xrand)
-            xnew     = steer(self,xnearest,xrand)
-            if not isCollide(self,xnearest,xnew):
-                self.V.append(xnew) # add point
-                self.wireup(xnew,xnearest)
-                #visualization(self)
+        while ind < self.maxiter and getDist(xnew, self.env.goal) > 1:
+            xrand = sampleFree(self)
+            xnearest = nearest(self, xrand)
+            xnew = steer(self, xnearest, xrand)
+            if not isCollide(self, xnearest, xnew):
+                self.V.append(xnew)  # add point
+                self.wireup(xnew, xnearest)
+                # visualization(self)
                 self.i += 1
             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 getDist(xnew, self.env.goal) <= 1:
+                self.wireup(self.env.goal, xnew)
+                self.Path, D = path(self)
+                print('Total distance = ' + str(D))
         visualization(self)
 
+
 if __name__ == '__main__':
     p = rrtstar()
     starttime = time.time()
     p.run()
-    print('time used = ' + str(time.time()-starttime))
+    print('time used = ' + str(time.time() - starttime))

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -1,4 +1,3 @@
-
 """
 This is rrt star code for 3D
 @author: yue qi
@@ -10,12 +9,12 @@ import time
 
 import os
 import sys
-sys.path.append(os.path.dirname(os.path.abspath(__file__))+"/../../Sampling-based Planning/")
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
 from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
 
 
-
 class rrtstar():
     def __init__(self):
         self.env = env()
@@ -27,59 +26,60 @@ class rrtstar():
         self.stepsize = 0.5
         self.Path = []
 
-    def wireup(self,x,y):
-        self.E.add_edge([x,y]) # add edge
+    def wireup(self, x, y):
+        self.E.add_edge([x, y])  # add edge
         self.Parent[str(x[0])][str(x[1])][str(x[2])] = y
 
-    def removewire(self,xnear):
+    def removewire(self, xnear):
         xparent = self.Parent[str(xnear[0])][str(xnear[1])][str(xnear[2])]
-        a = [xnear,xparent]
-        self.E.remove_edge(a) # remove and replace old the connection
+        a = [xnear, xparent]
+        self.E.remove_edge(a)  # remove and replace old the connection
 
     def run(self):
         self.V.append(self.env.start)
         ind = 0
         xnew = self.env.start
-        while ind < self.maxiter and getDist(xnew,self.env.goal) > 1:
-        #while ind < self.maxiter:
-            xrand    = sampleFree(self)
-            xnearest = nearest(self,xrand)
-            xnew     = steer(self,xnearest,xrand)
-            if not isCollide(self,xnearest,xnew):
-                Xnear = near(self,xnew)
-                self.V.append(xnew) # add point
+        while ind < self.maxiter and getDist(xnew, self.env.goal) > 1:
+            # while ind < self.maxiter:
+            xrand = sampleFree(self)
+            xnearest = nearest(self, xrand)
+            xnew = steer(self, xnearest, xrand)
+            if not isCollide(self, xnearest, xnew):
+                Xnear = near(self, xnew)
+                self.V.append(xnew)  # add point
                 # visualization(self)
                 # minimal path and minimal cost
-                xmin,cmin = xnearest,cost(self,xnearest) + getDist(xnearest,xnew)
+                xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew)
                 # connecting along minimal cost path
                 if self.i == 0:
-                    c1 = cost(self,Xnear) + getDist(xnew,Xnear)
-                    if not isCollide(self,xnew,Xnear) and c1 < cmin:
-                            xmin,cmin = Xnear,c1
-                    self.wireup(xnew,xmin)
+                    c1 = cost(self, Xnear) + getDist(xnew, Xnear)
+                    if not isCollide(self, xnew, Xnear) and c1 < cmin:
+                        xmin, cmin = Xnear, c1
+                    self.wireup(xnew, xmin)
                 else:
                     for xnear in Xnear:
-                        c1 = cost(self,xnear) + getDist(xnew,xnear)
-                        if not isCollide(self,xnew,xnear) and c1 < cmin:
-                            xmin,cmin = xnear,c1
-                    self.wireup(xnew,xmin)
+                        c1 = cost(self, xnear) + getDist(xnew, xnear)
+                        if not isCollide(self, xnew, xnear) and c1 < cmin:
+                            xmin, cmin = xnear, c1
+                    self.wireup(xnew, xmin)
                     # rewire
                     for xnear in Xnear:
-                        c2 = cost(self,xnew) + getDist(xnew,xnear)
-                        if not isCollide(self,xnew,xnear) and c2 < cost(self,xnear):
+                        c2 = cost(self, xnew) + getDist(xnew, xnear)
+                        if not isCollide(self, xnew, xnear) and c2 < cost(self, xnear):
                             self.removewire(xnear)
-                            self.wireup(xnear,xnew)
+                            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,self.D = path(self)
+            if getDist(xnew, self.env.goal) <= 1:
+                self.wireup(self.env.goal, xnew)
+                self.Path, self.D = path(self)
         visualization(self)
-        print('Total distance = '+str(self.D))
+        print('Total distance = ' + str(self.D))
+
 
 if __name__ == '__main__':
     p = rrtstar()
     starttime = time.time()
     p.run()
-    print('time used = ' + str(time.time()-starttime))
+    print('time used = ' + str(time.time() - starttime))