Merge branch 'master' of https://github.com/zhm-real/PathPlanning

Sampling_based_Planning/rrt_3D/FMT_star3D.py

@@ -0,0 +1,82 @@
+"""
+This is fast marching tree* code for 3D
+@author: yue qi 
+source: Janson, Lucas, et al. "Fast marching tree: A fast marching sampling-based method 
+        for optimal motion planning in many dimensions." 
+        The International journal of robotics research 34.7 (2015): 883-921.
+"""
+import numpy as np
+import matplotlib.pyplot as plt
+import time
+import copy
+
+
+import os
+import sys
+
+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
+
+class FMT_star:
+
+    def __init__(self):
+        self.env = env()
+        # note that the xgoal could be a region since this algorithm is a multiquery method
+        self.xinit, self.xgoal = tuple(self.env.start), tuple(self.env.goal)
+        self.n = 100 # number of samples
+        # sets
+        self.V = self.generateSampleSet(self.n - 2) # set of all nodes
+        self.Vopen = set(self.xinit) # open set
+        self.Vclosed = set() # closed set
+        self.Vunvisited = copy.deepcopy(self.V) # unvisited set
+        self.Vunvisited.add(self.xgoal)
+        # cost to come
+        self.c = {}
+
+    def generateSampleSet(self, n):
+        V = set()
+        for i in range(n):
+            V.add(sampleFree(self, bias = 0.0))
+        return V
+
+    def Near(self, nodeset, node, range):
+        newSet = set()
+        return newSet
+
+    def Path(self, T):
+        V, E = T
+        path = []
+        return path
+
+    def Cost(self, x, y):
+        pass
+
+    def FMTrun(self):
+        z = copy.deepcopy(self.xinit)
+        Nz = self.Near(self.Vunvisited, z, rn)
+        E = set()
+        # Save(Nz, z)
+        while z != self.xgoal:
+            Vopen_new = set()
+            Xnear = Nz.intersection(self.Vunvisited)
+            for x in Xnear:
+                Nx = self.Near(self.V.difference(set(x)), x, rn)
+                # Save(Nx, x)
+                Ynear = Nx.intersection(self.Vopen)
+                ymin = Ynear[np.argmin([self.c[y] + self.Cost(y,x) for y in Ynear])] # DP programming equation
+                collide, _ = self.isCollide(ymin, x)
+                if not collide:
+                    E = E.add((ymin, x)) # straight line joining ymin and x is collision free
+                    Vopen_new.add(x)
+                    self.Vunvisited = self.Vunvisited.difference(set(x))
+                    self.c[x] = self.c[ymin] + self.Cost(ymin, x) # cost-to-arrive from xinit in tree T = (VopenUVclosed, E)
+            self.Vopen = (self.Vopen.union(Vopen_new)).difference(set(z))
+            self.Vclosed = self.Vclosed.union(set(z))
+            if len(self.Vopen) > 0:
+                return 'Failure'
+            z = np.argmin([self.c[y] for y in self.Vopen])
+        return self.Path(z, T = (self.Vopen.union(self.Vclosed), E))
+
+
+

Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py

@@ -3,9 +3,6 @@ This is dynamic rrt code for 3D
 @author: yue qi
 """
 import numpy as np
-from numpy.matlib import repmat
-from collections import defaultdict
-import copy
 import time
 import matplotlib.pyplot as plt
 
@@ -14,9 +11,7 @@ import sys
 
 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, cost, path, edgeset, isinbound, \
-    isinside
-from rrt_3D.rrt3D import rrt
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide
 from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 
 
@@ -60,7 +55,6 @@ class dynamic_rrt_3D:
                 S.append(qi)
             i += 1
         self.CreateTreeFromNodes(S)
-        print('trimming complete...')
 
     def InvalidateNodes(self, obstacle):
         Edges = self.FindAffectedEdges(obstacle)
@@ -74,7 +68,7 @@ class dynamic_rrt_3D:
         self.flag[self.x0] = 'Valid'
 
     def GrowRRT(self):
-        print('growing')
+        print('growing...')
         qnew = self.x0
         distance_threshold = self.stepsize
         self.ind = 0
@@ -91,7 +85,6 @@ class dynamic_rrt_3D:
                 self.i += 1
             self.ind += 1
             # self.visualization()
-        print('growing complete...')
 
     def ChooseTarget(self):
         # return the goal, or randomly choose a state in the waypoints based on probs
@@ -141,7 +134,7 @@ class dynamic_rrt_3D:
         t = 0
         while True:
             # move the block while the robot is moving
-            new, _ = self.env.move_block(a=[0, 0, -0.2], mode='translation')
+            new, _ = self.env.move_block(a=[0.2, 0, -0.2], mode='translation')
             self.InvalidateNodes(new)
             self.TrimRRT()
             # if solution path contains invalid node
@@ -164,7 +157,7 @@ class dynamic_rrt_3D:
     def FindAffectedEdges(self, obstacle):
         # scan the graph for the changed edges in the tree.
         # return the end point and the affected
-        print('finding affected edges')
+        print('finding affected edges...')
         Affectededges = []
         for e in self.Edge:
             child, parent = e
@@ -177,7 +170,7 @@ class dynamic_rrt_3D:
         return edge[0]
 
     def CreateTreeFromNodes(self, Nodes):
-        print('creating tree')
+        print('creating tree...')
         # self.Parent = {node: self.Parent[node] for node in Nodes}
         self.V = [node for node in Nodes]
         self.Edge = {(node, self.Parent[node]) for node in Nodes}

Sampling_based_Planning/rrt_3D/informed_rrt_star3D.py

@@ -0,0 +1 @@
+# informed RRT star in 3D