'OBB_line'

Search-based Planning/Search_3D/DstarLite3D.py

@@ -28,9 +28,9 @@ class D_star_Lite(object):
                         # (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
                         # (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
         self.env = env(resolution=resolution)
-        self.X = StateSpace(self.env)
-        self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal)
-        #self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
+        #self.X = StateSpace(self.env)
+        #self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal)
+        self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
         # self.OPEN = queue.QueuePrior()
         self.OPEN = queue.MinheapPQ()
         self.km = 0
@@ -64,7 +64,7 @@ class D_star_Lite(object):
     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:
+        for xi in self.CLOSED:
             if xi in self.CHILDREN:
                 oldchildren = self.CHILDREN[xi]# A
                 if isinbound(old, xi, mode) or isinbound(new, xi, mode):
@@ -77,7 +77,6 @@ class D_star_Lite(object):
                     for xj in intersection.union(added):
                         self.COST[xi][xj] = cost(self, xi, xj)
                     CHANGED.add(xi)
-                    self.CHILDREN[xi] = newchildren
             else: 
                 if isinbound(old, xi, mode) or isinbound(new, xi, mode):
                     CHANGED.add(xi)
@@ -116,8 +115,10 @@ class D_star_Lite(object):
     def UpdateVertex(self, u):
         # if still in the hunt
         if not getDist(self.xt, u) <= self.env.resolution: # originally: u != s_goal
-            cdren = self.getchildren(u)
-            self.rhs[u] = min([self.getcost(s, u) + self.getg(s) for s in self.getchildren(u)])
+            if u in self.CHILDREN and len(self.CHILDREN[u]) == 0:
+                self.rhs[u] = np.inf
+            else:
+                self.rhs[u] = min([self.getcost(s, u) + self.getg(s) for s in self.getchildren(u)])
         # if u is in OPEN, remove it
         self.OPEN.check_remove(u)
         # if rhs(u) not equal to g(u)
@@ -130,10 +131,10 @@ class D_star_Lite(object):
             u = self.OPEN.get()
             self.V.add(u)
             self.CLOSED.add(u)
-            # if not self.done: # first time running, we need to stop on this condition
-            #     if getDist(self.x0,u) < 1.5*self.env.resolution:
-            #         self.x0 = u
-            #         break
+            if not self.done: # first time running, we need to stop on this condition
+                if getDist(self.x0,u) < 1*self.env.resolution:
+                    self.x0 = u
+                    break
             if kold < self.CalculateKey(u):
                 self.OPEN.put(u, self.CalculateKey(u))
             if self.getg(u) > self.getrhs(u):
@@ -143,6 +144,7 @@ class D_star_Lite(object):
                 self.UpdateVertex(u)
             for s in self.getchildren(u):
                 self.UpdateVertex(s)
+            # visualization(self)
             self.ind += 1
 
     def main(self):
@@ -159,17 +161,14 @@ class D_star_Lite(object):
         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 % 1 == 0: 
-                # new1,old1 = self.env.move_block(a=[0, 0, -2], s=0.5, block_to_move=0, mode='translation')
+            #---------------------------------- at specific times, the environment is changed and cost is updated
+            if t % 2 == 0: 
+                new0,old0 = self.env.move_block(a=[-0.1, 0, -0.2], s=0.5, block_to_move=1, 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 = []
-            # if t == 5: 
-            #     new1,old1 = self.env.move_block(a=[0, 0, -1], s=0.5, block_to_move=1, mode='translation')
-            #     ischanged = True
             #----------------------------------- traverse the route as originally planned
             if t == 0:
                 children_new = [i for i in self.CLOSED if getDist(self.x0, i) <= self.env.resolution*np.sqrt(3)]
@@ -184,16 +183,16 @@ class D_star_Lite(object):
             if ischanged:
                 self.km += heuristic_fun(self, self.x0, s_last)
                 s_last = self.x0
-                CHANGED = self.updatecost(True, new1, old1)
+                CHANGED = self.updatecost(True, new0, old0)
+                CHANGED1 = self.updatecost(True, new1, old1)
                 CHANGED2 = self.updatecost(True, new2, old2, mode='obb')
-                CHANGED = CHANGED.union(CHANGED2)
+                CHANGED = CHANGED.union(CHANGED1, CHANGED2)
                 # self.V = set()
                 for u in CHANGED:
                     self.UpdateVertex(u)
                 self.ComputeShortestPath()
                 
                 ischanged = False
-            # self.Path = self.path(s_start)
             self.Path = self.path(self.x0)
             visualization(self)
             t += 1
@@ -217,7 +216,7 @@ class D_star_Lite(object):
                 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([cost(self,s,s_p) + self.getg(s_p) for s_p in children])]
+            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:

Search-based Planning/Search_3D/env3D.py

@@ -59,6 +59,7 @@ class obb(object):
         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
@@ -85,7 +86,7 @@ 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.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 = []
@@ -154,6 +155,7 @@ class env():
         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]
           
 

Search-based Planning/Search_3D/plot_util3D.py

@@ -91,7 +91,7 @@ def visualization(initparams):
         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=8., azim=105.)
+        ax.view_init(elev=8., azim=120.)
         #ax.view_init(elev=-8., azim=180)
         ax.clear()
         # drawing objects

Search-based Planning/Search_3D/utils3D.py

@@ -52,12 +52,9 @@ def isinball(i, x):
     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]
+    # transform the point from {W} to {body}
+    pt = i.T@np.append(x,1)
+    block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]]
     return isinbound(block, pt)
 
 def OBB2AABB(obb):
@@ -100,30 +97,33 @@ def lineAABB(p0, p1, dist, aabb):
     mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2]  # mid point
     I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist]  # unit direction
     hl = dist / 2  # radius
-    P = aabb.P  # center of the AABB
-    E = aabb.E  # extents of AABB
-    T = [P[0] - mid[0], P[1] - mid[1], P[2] - mid[2]]
+    T = [aabb.P[0] - mid[0], aabb.P[1] - mid[1], aabb.P[2] - mid[2]]
     # do any of the principal axis form a separting axis?
-    if abs(T[0]) > (E[0] + hl * abs(I[0])): return False
-    if abs(T[1]) > (E[1] + hl * abs(I[1])): return False
-    if abs(T[2]) > (E[2] + hl * abs(I[2])): return False
+    if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False
+    if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False
+    if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False
     # I.cross(x axis) ?
-    r = E[1] * abs(I[2]) + E[2] * abs(I[1])
+    r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1])
     if abs(T[1] * I[2] - T[2] * I[1]) > r: return False
     # I.cross(y axis) ?
-    r = E[0] * abs(I[2]) + E[2] * abs(I[0])
+    r = aabb.E[0] * abs(I[2]) + aabb.E[2] * abs(I[0])
     if abs(T[2] * I[0] - T[0] * I[2]) > r: return False
     # I.cross(z axis) ?
-    r = E[0] * abs(I[1]) + E[1] * abs(I[0])
+    r = aabb.E[0] * abs(I[1]) + aabb.E[1] * abs(I[0])
     if abs(T[0] * I[1] - T[1] * I[0]) > r: return False
 
     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)
+    # transform points to obb frame
+    res = obb.T@np.column_stack([np.array([p0,p1]),[1,1]]).T 
+    # record old position and set the position to origin
+    oldP, obb.P= obb.P, [0,0,0] 
+    # calculate segment-AABB testing
+    ans = lineAABB(res[0:3,0],res[0:3,1],dist,obb)
+    # reset the position
+    obb.P = oldP 
+    return ans
 
 def OBBOBB(obb1, obb2):
     # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
@@ -311,7 +311,12 @@ def obstacleFree(initparams, x):
 
 
 def cost(initparams, i, j, dist=None, settings='Euclidean'):
-    collide, dist = isCollide(initparams, i, j, dist)
+    if initparams.env.resolution < 0.25:
+        if dist==None:
+            dist = getDist(i,j)
+        collide = False
+    else:
+        collide, dist = isCollide(initparams, i, j, dist)
     # collide, dist= False, getDist(i, j)
     if settings == 'Euclidean':
         if collide:
@@ -335,7 +340,17 @@ def initcost(initparams):
     return c
 
 if __name__ == "__main__":
-    pass
-    # obb1 = obb([0,0,0],[1,1,1],[[1,0,0],[0,1,0],[0,0,1]])
+    import time
+    from env3D import R_matrix, obb
+    obb1 = obb([2.6,2.5,1],[0.2,2,2],R_matrix(0,0,45))
     # 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))
+    p0, p1 = [2.9,2.5,1],[1.9,2.5,1]
+    dist = getDist(p0,p1)
+    start = time.time()
+    for i in range(3000*27):
+        lineAABB(p0,p1,dist,obb1)
+        #lineOBB(p0,p1,dist,obb1)
+    print(time.time() - start)
+    
+
+