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

Search-based Planning/Search_3D/Astar3D.py

@@ -114,7 +114,7 @@ class Weighted_A_star(object):
 
 if __name__ == '__main__':
     
-    Astar = Weighted_A_star(1)
+    Astar = Weighted_A_star(0.5)
     sta = time.time()
     Astar.run()
     print(time.time() - sta)

Search-based Planning/Search_3D/DstarLite3D.py

@@ -61,13 +61,13 @@ class D_star_Lite(object):
             self.COST[xi][xj] = cost(self, xi, xj)
         return self.COST[xi][xj]
 
-    def updatecost(self,range_changed=None, new=None, old=None):
+    def updatecost(self,range_changed=None, new=None, old=None, mode=False):
         # scan graph for changed cost, if cost is changed update it
         CHANGED = set()
         for xi in self.X:
             if xi in self.CHILDREN:
                 oldchildren = self.CHILDREN[xi]# A
-                if isinbound(old, xi) or isinbound(new, xi):
+                if isinbound(old, xi, mode) or isinbound(new, xi, mode):
                     newchildren = set(children(self,xi))# B
                     removed = oldchildren.difference(newchildren)
                     intersection = oldchildren.intersection(newchildren)
@@ -79,7 +79,7 @@ class D_star_Lite(object):
                     CHANGED.add(xi)
                     self.CHILDREN[xi] = newchildren
             else: 
-                if isinbound(old, xi) or isinbound(new, xi):
+                if isinbound(old, xi, mode) or isinbound(new, xi, mode):
                     CHANGED.add(xi)
                     children_added = set(children(self,xi))
                     self.CHILDREN[xi] = children_added
@@ -157,11 +157,13 @@ class D_star_Lite(object):
         print('running with map update ...')
         t = 0 # count time
         ischanged = False
+        self.V = set()
         while getDist(self.x0, self.xt) > 2*self.env.resolution:
             #---------------------------------- at 5th node, the environment is changed and cost is updated
-            if t % 2 == 0: 
+            if t % 1 == 0: 
                 # new1,old1 = self.env.move_block(a=[0, 0, -2], s=0.5, block_to_move=0, mode='translation')
-                new1,old1 = self.env.move_block(a=[0, 0, -0.1], s=0.5, block_to_move=0, mode='translation')
+                new1,old1 = self.env.move_block(a=[0, 0, -0.2], s=0.5, block_to_move=0, mode='translation')
+                new2,old2 = self.env.move_block(theta = [0,0,0.1*t], mode='rotation')
                 #new2,old2 = self.env.move_block(a=[-0.3, 0, -0.1], s=0.5, block_to_move=1, mode='translation')
                 ischanged = True
                 self.Path = []
@@ -183,9 +185,9 @@ class D_star_Lite(object):
                 self.km += heuristic_fun(self, self.x0, s_last)
                 s_last = self.x0
                 CHANGED = self.updatecost(True, new1, old1)
-                #CHANGED2 = self.updatecost(True, new2, old2)
-                #CHANGED = CHANGED.union(CHANGED2)
-                self.V = set()
+                CHANGED2 = self.updatecost(True, new2, old2, mode='obb')
+                CHANGED = CHANGED.union(CHANGED2)
+                # self.V = set()
                 for u in CHANGED:
                     self.UpdateVertex(u)
                 self.ComputeShortestPath()

Search-based Planning/Search_3D/env3D.py

@@ -5,7 +5,19 @@
 @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():
     # AABBs
@@ -30,9 +42,23 @@ def getAABB(blocks):
     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
 
 def getAABB2(blocks):
     # used in lineAABB
@@ -59,6 +85,10 @@ class env():
         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,1,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 
@@ -72,9 +102,9 @@ class env():
     def move_start(self, x):
         self.start = x
 
-    def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], G = None, block_to_move = 0, mode = 'uniform'):
+    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, 
-        # G is an orthorgonal transform in R3*3, in the Galilean transformation
+        # 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])
@@ -120,13 +150,13 @@ class env():
                     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') = (Gx, t)
-        if mode == 'rotation': # this makes AABB become a OBB
-            #TODO: implement this with rotation matrix
-            pass
+        # (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])
+            return self.OBB[obb_to_move], ori[0]
           
 
 
 if __name__ == '__main__':
     newenv = env()
-    print(newenv.balls)

Search-based Planning/Search_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:
@@ -62,11 +90,15 @@ def visualization(initparams):
         # generate axis objects
         ax = plt.subplot(111, projection='3d')
         #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=0., azim=90.)
+        ax.view_init(elev=8., azim=105.)
+        #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')
@@ -136,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

Search-based Planning/Search_3D/utils3D.py

@@ -1,6 +1,7 @@
 import numpy as np
 import pyrr
 from collections import defaultdict
+import copy
 
 
 def getRay(x, y):
@@ -38,17 +39,42 @@ def heuristic_fun(initparams, k, t=None):
         t = initparams.goal
     return max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])])
 
-def isinbound(i, x):
+def isinbound(i, x, mode=False):
+    if mode == 'obb':
+        return isinobb(i, x)
     if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]:
         return True
     return False
 
-
 def isinball(i, x):
     if getDist(i[0:3], x) <= i[3]:
         return True
     return False
 
+def isinobb(i, x):
+    # pt = i.O.T@np.array(x) # transform the point from {W} to {body}
+    # minx,miny,minz,maxx,maxy,maxz = i.P[0] - i.E[0],i.P[1] - i.E[1],i.P[2] - i.E[2],i.P[0] + i.E[0],i.P[1] + i.E[1],i.P[2] + i.E[2]
+    T = np.vstack([np.column_stack([i.O.T,-i.O.T@i.P]),[0,0,0,1]])
+    pt = T@np.append(x,1) # transform the point from {W} to {body}
+    minx,miny,minz,maxx,maxy,maxz = - i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]
+    block = [minx,miny,minz,maxx,maxy,maxz]
+    return isinbound(block, pt)
+
+def OBB2AABB(obb):
+    # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
+    aabb = copy.deepcopy(obb)
+    P = obb.P
+    a = obb.E
+    A = obb.O
+    # a1(A1 dot x) + a2(A2 dot x) + a3(A3 dot x) 
+    Ex = a[0]*abs(A[0][0]) + a[1]*abs(A[1][0]) + a[2]*abs(A[2][0])
+    Ey = a[0]*abs(A[0][1]) + a[1]*abs(A[1][1]) + a[2]*abs(A[2][1])
+    Ez = a[0]*abs(A[0][2]) + a[1]*abs(A[1][2]) + a[2]*abs(A[2][2])
+    E = np.array([Ex, Ey, Ez])
+    aabb.P = P
+    aabb.E = E
+    aabb.O = np.array([[1,0,0],[0,1,0],[0,0,1]])
+    return aabb
 
 def lineSphere(p0, p1, ball):
     # https://cseweb.ucsd.edu/classes/sp19/cse291-d/Files/CSE291_13_CollisionDetection.pdf
@@ -68,7 +94,6 @@ def lineSphere(p0, p1, ball):
         if (k[0] * k[0] + k[1] * k[1] + k[2] * k[2]) <= r ** 2: return True
     return False
 
-
 def lineAABB(p0, p1, dist, aabb):
     # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
     # aabb should have the attributes of P, E as center point and extents
@@ -94,6 +119,12 @@ def lineAABB(p0, p1, dist, aabb):
 
     return True
 
+def lineOBB(p0, p1, dist, obb):
+    T = np.vstack([np.column_stack([obb.O.T,-obb.O.T@obb.P]),[0,0,0,1]])
+    p0 = T@np.append(p0,1) # transform the points to the box
+    p1 = T@np.append(p1,1)
+    return lineAABB(p0[0:3],p1[0:3],dist,obb)
+
 def OBBOBB(obb1, obb2):
     # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
     # each obb class should contain attributes:
@@ -147,7 +178,6 @@ def OBBOBB(obb1, obb2):
             #L = A0 x B2
             ra = a[1]*abs(R[2][2]) + a[2]*abs(R[1][2])
             rb = b[0]*abs(R[0][1]) + b[1]*abs(R[0][0])
-
             t = abs(T[2]*R[1][2] - T[1]*R[2][2])
             if t > ra + rb:
                 return False
@@ -155,7 +185,6 @@ def OBBOBB(obb1, obb2):
             #L = A1 x B0
             ra = a[0]*abs(R[2][0]) + a[2]*abs(R[0][0])
             rb = b[1]*abs(R[1][2]) + b[2]*abs(R[1][1])
-
             t = abs( T[0]*R[2][0] - T[2]*R[0][0] )
             if t > ra + rb:
                 return False
@@ -198,9 +227,6 @@ def OBBOBB(obb1, obb2):
             # no separating axis found,
             # the two boxes overlap 
             return True
-    
-
-
 
 def StateSpace(env, factor=0):
     boundary = env.boundary
@@ -234,18 +260,21 @@ def isCollide(initparams, x, child, dist):
     '''specified for expansion in A* 3D lookup table'''
     if dist==None:
         dist = getDist(x, child)
+    # check in bound
     if not isinbound(initparams.env.boundary, child): 
         return True, dist
+    # check collision in AABB
     for i in range(len(initparams.env.AABB)):
-        # if isinbound(initparams.env.blocks[i], child): 
-        #     return True, dist
         if lineAABB(x, child, dist, initparams.env.AABB[i]): 
             return True, dist
+    # check collision in ball
     for i in initparams.env.balls:
-        # if isinball(i, child): 
-        #     return True, dist
         if lineSphere(x, child, i): 
             return True, dist
+    # check collision with obb
+    for i in initparams.env.OBB:
+        if lineOBB(x, child, dist, i):
+            return True, dist
     return False, dist
 
 
@@ -256,6 +285,8 @@ def children(initparams, x, settings = 0):
     resolution = initparams.env.resolution
     for direc in initparams.Alldirec:
         child = tuple(map(np.add, x, np.multiply(direc, resolution)))
+        if any([isinobb(i, child) for i in initparams.env.OBB]):
+            continue
         if any([isinball(i ,child) for i in initparams.env.balls]):
             continue
         if any([isinbound(i ,child) for i in initparams.env.blocks]):
@@ -303,13 +334,8 @@ def initcost(initparams):
             c[xi][child] = cost(initparams, xi, child)
     return c
 
-class obb(object):
-    def __init__(self, P, E, O):
-        self.P = P
-        self.E = E
-        self.O = O
-
 if __name__ == "__main__":
-    obb1 = obb([0,0,0],[1,1,1],[[1,0,0],[0,1,0],[0,0,1]])
-    obb2 = obb([1,1,0],[1,1,1],[[1/np.sqrt(3)*1,1/np.sqrt(3)*1,1/np.sqrt(3)*1],[np.sqrt(3/2)*(-1/3),np.sqrt(3/2)*2/3,np.sqrt(3/2)*(-1/3)],[np.sqrt(1/8)*(-2),0,np.sqrt(1/8)*2]])
-    print(OBBOBB(obb1, obb2))
+    pass
+    # obb1 = obb([0,0,0],[1,1,1],[[1,0,0],[0,1,0],[0,0,1]])
+    # obb2 = obb([1,1,0],[1,1,1],[[1/np.sqrt(3)*1,1/np.sqrt(3)*1,1/np.sqrt(3)*1],[np.sqrt(3/2)*(-1/3),np.sqrt(3/2)*2/3,np.sqrt(3/2)*(-1/3)],[np.sqrt(1/8)*(-2),0,np.sqrt(1/8)*2]])
+    # print(OBBOBB(obb1, obb2))