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

Sampling-based Planning/rrt_3D/env3D.py

@@ -5,9 +5,21 @@
 @author: yue qi
 """
 import numpy as np
+# from utils3D import OBB2AABB
 
+def R_matrix(z_angle,y_angle,x_angle):
+    # x angle: row; y angle: pitch; z angle: yaw
+    # generate rotation matrix in SO3
+    # RzRyRx = R, ZYX intrinsic rotation
+    # also (r1,r2,r3) in R3*3 in {W} frame
+    # used in obb.O
+    # [[R p]
+    # [0T 1]] gives transformation from body to world 
+    return np.array([[np.cos(z_angle), -np.sin(z_angle), 0.0], [np.sin(z_angle), np.cos(z_angle), 0.0], [0.0, 0.0, 1.0]])@ \
+           np.array([[np.cos(y_angle), 0.0, np.sin(y_angle)], [0.0, 1.0, 0.0], [-np.sin(y_angle), 0.0, np.cos(y_angle)]])@ \
+           np.array([[1.0, 0.0, 0.0], [0.0, np.cos(x_angle), -np.sin(x_angle)], [0.0, np.sin(x_angle), np.cos(x_angle)]])
 
-def getblocks(resolution):
+def getblocks():
     # AABBs
     block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00],
              [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00],
@@ -19,31 +31,134 @@ def getblocks(resolution):
     Obstacles = []
     for i in block:
         i = np.array(i)
-        Obstacles.append([j/resolution for j in i])
+        Obstacles.append([j for j in i])
     return np.array(Obstacles)
 
-def getballs(resolution):
-    spheres = [[16,2.5,3,2],[10,2.5,1,1]]
+def getAABB(blocks):
+    # used for Pyrr package for detecting collision
+    AABB = []
+    for i in blocks:
+        AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)]))  # make AABBs alittle bit of larger
+    return AABB
+
+class aabb(object):
+    # make AABB out of blocks, 
+    # P: center point
+    # E: extents
+    # O: Rotation matrix in SO(3), in {w}
+    def __init__(self,AABB):
+        self.P = [(AABB[3] + AABB[0])/2, (AABB[4] + AABB[1])/2, (AABB[5] + AABB[2])/2]# center point
+        self.E = [(AABB[3] - AABB[0])/2, (AABB[4] - AABB[1])/2, (AABB[5] - AABB[2])/2]# extents
+        self.O = [[1,0,0],[0,1,0],[0,0,1]]
+
+class obb(object):
+    # P: center point
+    # E: extents
+    # O: Rotation matrix in SO(3), in {w}
+    def __init__(self, P, E, O):
+        self.P = P
+        self.E = E
+        self.O = O
+        self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]])
+
+def getAABB2(blocks):
+    # used in lineAABB
+    AABB = []
+    for i in blocks:
+        AABB.append(aabb(i))
+    return AABB
+
+def getballs():
+    spheres = [[16,2.5,4,2],[10,2.5,1,1]]
     Obstacles = []
     for i in spheres:
-        Obstacles.append([j/resolution for j in i])
+        Obstacles.append([j for j in i])
     return np.array(Obstacles)
 
+def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]):
+    return block
+
 class env():
     def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1):
         self.resolution = resolution
-        self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) / resolution
-        self.blocks = getblocks(resolution)
-        self.balls = getballs(resolution)
+        self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) 
+        self.blocks = getblocks()
+        self.AABB = getAABB2(self.blocks)
+        self.AABB_pyrr = getAABB(self.blocks)
+        self.balls = getballs()
+        self.OBB = np.array([obb([2.6,2.5,1],[0.2,2,1],R_matrix(0,0,45))])
+        #self.OBB = np.squeeze(np.vstack([self.OBB,OBB2AABB(self.OBB[0])]))
+        #print(self.OBB)
+        # self.OBB = []
         self.start = np.array([0.5, 2.5, 5.5])
         self.goal = np.array([19.0, 2.5, 5.5])
+        self.t = 0 # time 
+
+    def New_block(self):
+        newblock = add_block()
+        self.blocks = np.vstack([self.blocks,newblock])
+        self.AABB = getAABB2(self.blocks)
+        self.AABB_pyrr = getAABB(self.blocks)
+
+    def move_start(self, x):
+        self.start = x
+
+    def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], theta = [0,0,0], block_to_move = 0, obb_to_move = 0, mode = 'uniform'):
+        # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, 
+        # R is an orthorgonal transform in R3*3, is the rotation matrix
+        # (x',t') = (x + tv, t) is uniform transformation
+        if mode == 'uniform':
+            ori = np.array(self.blocks[block_to_move])
+            self.blocks[block_to_move] = \
+                np.array([ori[0] + self.t * v[0],\
+                    ori[1] + self.t * v[1],\
+                    ori[2] + self.t * v[2],\
+                    ori[3] + self.t * v[0],\
+                    ori[4] + self.t * v[1],\
+                    ori[5] + self.t * v[2]])
+
+            self.AABB[block_to_move].P = \
+            [self.AABB[block_to_move].P[0] + self.t * v[0], \
+            self.AABB[block_to_move].P[1] + self.t * v[1], \
+            self.AABB[block_to_move].P[2] + self.t * v[2]]
+            # return a range of block that the block might moved
+            a = self.blocks[block_to_move]
+            # return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \
+            #                 a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]). \
+                    # np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \
+                    #         ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
+            return a,ori
+        # (x',t') = (x + a, t + s) is a translation
+        if mode == 'translation':
+            ori = np.array(self.blocks[block_to_move])
+            self.blocks[block_to_move] = \
+                np.array([ori[0] + a[0],\
+                    ori[1] + a[1],\
+                    ori[2] + a[2],\
+                    ori[3] + a[0],\
+                    ori[4] + a[1],\
+                    ori[5] + a[2]])
 
-    def visualize(self):
-        # fig = plt.figure()
-        # TODO: do visualizations
-        return
+            self.AABB[block_to_move].P = \
+            [self.AABB[block_to_move].P[0] + a[0], \
+            self.AABB[block_to_move].P[1] + a[1], \
+            self.AABB[block_to_move].P[2] + a[2]]
+            self.t += s
+            # return a range of block that the block might moved
+            a = self.blocks[block_to_move]
+            return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \
+                            a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \
+                    np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \
+                            ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
+            # return a,ori
+        # (x',t') = (Rx, t)
+        if mode == 'rotation': # this makes an OBB rotate
+            ori = [self.OBB[obb_to_move]]
+            self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2])
+            self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,-self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[0,0,0,1]])
+            return self.OBB[obb_to_move], ori[0]
+          
 
 
 if __name__ == '__main__':
     newenv = env()
-    print(newenv.balls)

Sampling-based Planning/rrt_3D/plot_util3D.py

@@ -42,6 +42,34 @@ def draw_block_list(ax, blocks ,color=None,alpha=0.15):
         h = ax.add_collection3d(pc)
         return h
 
+def obb_verts(obb):
+    # 0.017004013061523438 for 1000 iters
+    ori_body = np.array([[1,1,1],[-1,1,1],[-1,-1,1],[1,-1,1],\
+                [1,1,-1],[-1,1,-1],[-1,-1,-1],[1,-1,-1]])
+    # P + (ori * E)
+    ori_body = np.multiply(ori_body,obb.E)
+    # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3)
+    verts = (obb.O@ori_body.T).T + obb.P
+    return verts
+
+
+def draw_obb(ax, OBB, color=None,alpha=0.15):
+    f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]])
+    n = OBB.shape[0]
+    vl = np.zeros((8 * n, 3))
+    fl = np.zeros((6 * n, 4), dtype='int64')
+    for k in range(n):
+        vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k])
+        fl[k * 6:(k + 1) * 6, :] = f + k * 8
+    if type(ax) is Poly3DCollection:
+        ax.set_verts(vl[fl])
+    else:
+        pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k')
+        pc.set_facecolor(color)
+        h = ax.add_collection3d(pc)
+        return h
+
+
 def draw_line(ax,SET,visibility=1,color=None):
     if SET != []:
         for i in SET:
@@ -52,8 +80,8 @@ def draw_line(ax,SET,visibility=1,color=None):
             ax.add_line(line)
 
 def visualization(initparams):
-    if initparams.ind % 10 == 0 or initparams.done:
-        V = np.array(initparams.V)
+    if initparams.ind % 20 == 0 or initparams.done:
+        V = np.array(list(initparams.V))
         E = initparams.E
         Path = np.array(initparams.Path)
         start = initparams.env.start
@@ -61,15 +89,21 @@ def visualization(initparams):
         edges = E.get_edge()
         # generate axis objects
         ax = plt.subplot(111, projection='3d')
-        ax.view_init(elev=0., azim=90)
+        #ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
+        #ax.view_init(elev=0., azim=90.)
+        ax.view_init(elev=8., azim=120.)
+        #ax.view_init(elev=-8., azim=180)
         ax.clear()
         # drawing objects
         draw_Spheres(ax, initparams.env.balls)
         draw_block_list(ax, initparams.env.blocks)
+        if initparams.env.OBB is not None:
+            draw_obb(ax,initparams.env.OBB)
         draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0)
         draw_line(ax,edges,visibility=0.25)
         draw_line(ax,Path,color='r')
-        ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',)
+        if len(V) > 0:
+            ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',)
         ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
         ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') 
         # adjust the aspect ratio
@@ -80,7 +114,7 @@ def visualization(initparams):
         ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy)
         plt.xlabel('x')
         plt.ylabel('y')
-        plt.pause(0.001)
+        plt.pause(0.0001)
 
 def make_get_proj(self, rx, ry, rz):
     '''
@@ -134,4 +168,7 @@ def make_get_proj(self, rx, ry, rz):
         M0 = np.dot(viewM, np.dot(aspectM, worldM)) ##
         M = np.dot(perspM, M0)
         return M
-    return get_proj
+    return get_proj
+
+if __name__ == '__main__':
+    pass

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:

Search-based Planning/Search_3D/Anytime_Dstar3D.py

@@ -52,6 +52,7 @@ class Anytime_Dstar(object):
         # epsilon in the key caculation
         self.epsilon = 1
         self.increment = 0.1
+        self.decrement = 0.2
 
     def getcost(self, xi, xj):
         # use a LUT for getting the costd
@@ -131,9 +132,12 @@ class Anytime_Dstar(object):
                 self.INCONS.add(s)
 
     def ComputeorImprovePath(self):
-        while self.OPEN.top_key() < self.key(self.x0) or self.rhs[self.x0] != self.g[self.x0]:
+        while self.OPEN.top_key() < self.key(self.x0,self.epsilon) or self.rhs[self.x0] != self.g[self.x0]:
             s = self.OPEN.get()
-            visualization(self)
+
+            if getDist(s, tuple(self.env.start)) < self.env.resolution:
+                break
+
             if self.g[s] > self.rhs[s]:
                 self.g[s] = self.rhs[s]
                 self.CLOSED.add(s)
@@ -148,45 +152,77 @@ class Anytime_Dstar(object):
             self.ind += 1
 
     def Main(self):
-        epsilon = self.epsilon
-        increment = self.increment
         ischanged = False
         islargelychanged = False
+        t = 0
         self.ComputeorImprovePath()
         #TODO publish current epsilon sub-optimal solution
+        self.done = True
+        self.ind = 0
+        self.Path = self.path()
+        visualization(self)
         while True:
+            visualization(self)
+            if t == 20:
+                break
             # change environment
-            new2,old2 = self.env.move_block(theta = [0,0,0.1*t], mode='rotation')
+            # new2,old2 = self.env.move_block(theta = [0,0,0.1*t], mode='rotation')
+            new2,old2 = self.env.move_block(a = [0,0,-0.2], mode='translation')
             ischanged = True
             # islargelychanged = True
             self.Path = []
 
             # update cost with changed environment
             if ischanged:
-                CHANGED = self.updatecost(True, new2, old2, mode='obb')
+                # CHANGED = self.updatecost(True, new2, old2, mode='obb')
+                CHANGED = self.updatecost(True, new2, old2)
                 for u in CHANGED:
                     self.UpdateState(u)
                 self.ComputeorImprovePath()
                 ischanged = False
 
             if islargelychanged:  
-                epsilon += increment # or replan from scratch
-            elif epsilon > 1:
-                epsilon -= increment
+                self.epsilon += self.increment # or replan from scratch
+            elif self.epsilon > 1:
+                self.epsilon -= self.decrement
             
             # move states from the INCONS to OPEN
             # update priorities in OPEN
-            Allnodes = self.INCONS.union(set(self.OPEN.enumerate()))
+            Allnodes = self.INCONS.union(self.OPEN.allnodes())
             for node in Allnodes:
-                self.OPEN.put(node, self.key(node, epsilon)) 
+                self.OPEN.put(node, self.key(node, self.epsilon)) 
             self.INCONS = set()
             self.CLOSED = set()
             self.ComputeorImprovePath()
-            # publish current epsilon sub optimal solution
+            #TODO publish current epsilon sub optimal solution
+            self.Path = self.path()
             # if epsilon == 1:
                 # wait for change to occur
-        pass
+            t += 1
+
+    def path(self, s_start=None):
+        '''After ComputeShortestPath()
+        returns, one can then follow a shortest path from s_start to
+        s_goal by always moving from the current vertex s, starting
+        at s_start. , to any successor s' that minimizes c(s,s') + g(s') 
+        until s_goal is reached (ties can be broken arbitrarily).'''
+        path = []
+        s_goal = self.xt
+        s = self.x0
+        ind = 0
+        while getDist(s, s_goal) > self.env.resolution:
+            if s == self.x0:
+                children = [i for i in self.CLOSED if getDist(s, i) <= self.env.resolution*np.sqrt(3)]
+            else: 
+                children = list(self.CHILDREN[s])
+            snext = children[np.argmin([self.getcost(s,s_p) + self.getg(s_p) for s_p in children])]
+            path.append([s, snext])
+            s = snext
+            if ind > 100:
+                break
+            ind += 1
+        return path
 
 if __name__ == '__main__':
-    AD = Anytime_Dstar(resolution = 0.5)
+    AD = Anytime_Dstar(resolution = 1)
     AD.Main()

Search-based Planning/Search_3D/queue.py

@@ -76,6 +76,7 @@ class MinheapPQ:
     """
     def __init__(self):
         self.pq = [] # lis of the entries arranged in a heap
+        self.nodes = set()
         self.entry_finder = {} # mapping of the item entries
         self.counter = itertools.count() # unique sequence count
         self.REMOVED = '<removed-item>'
@@ -88,12 +89,14 @@ class MinheapPQ:
         entry = [priority, count, item]
         self.entry_finder[item] = entry
         heapq.heappush(self.pq, entry)
+        self.nodes.add(item)
 
     def check_remove(self, item):
         if item not in self.entry_finder:
             return
         entry = self.entry_finder.pop(item)
         entry[-1] = self.REMOVED
+        self.nodes.remove(item)
 
     def get(self):
         """Remove and return the lowest priority task. Raise KeyError if empty."""
@@ -101,6 +104,7 @@ class MinheapPQ:
             priority, count, item = heapq.heappop(self.pq)
             if item is not self.REMOVED:
                 del self.entry_finder[item]
+                self.nodes.remove(item)
                 return item
         raise KeyError('pop from an empty priority queue')
 
@@ -110,6 +114,9 @@ class MinheapPQ:
     def enumerate(self):
         return self.pq
 
+    def allnodes(self):
+        return self.nodes
+
 # class QueuePrior:
 #     """
 #     Class: QueuePrior