diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 938a22d..fa27d0f 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/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) \ No newline at end of file diff --git a/Search-based Planning/Search_3D/DstarLite3D.py b/Search-based Planning/Search_3D/DstarLite3D.py index 14f7ad8..7f766ab 100644 --- a/Search-based Planning/Search_3D/DstarLite3D.py +++ b/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 @@ -159,9 +159,10 @@ class D_star_Lite(object): ischanged = False 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 +184,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() diff --git a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc index 692c02f..12c4439 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc index abc4a38..ba3a7ce 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc index 3b5b75e..8e23f3c 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc index c9b85e7..048bc85 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/env3D.py b/Search-based Planning/Search_3D/env3D.py index c44cfd6..bebf1d0 100644 --- a/Search-based Planning/Search_3D/env3D.py +++ b/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) diff --git a/Search-based Planning/Search_3D/plot_util3D.py b/Search-based Planning/Search_3D/plot_util3D.py index e52a5a4..1293e5c 100644 --- a/Search-based Planning/Search_3D/plot_util3D.py +++ b/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 \ No newline at end of file + return get_proj + +if __name__ == '__main__': + pass \ No newline at end of file diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index d64dea6..1640ef1 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/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,39 @@ 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] + 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 +91,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 +116,11 @@ def lineAABB(p0, p1, dist, aabb): return True +def lineOBB(p0, p1, dist, obb): + p0 = obb.O.T@np.array(p0) + p1 = obb.O.T@np.array(p1) + return lineAABB(p0,p1,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 +174,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 +181,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 +223,6 @@ def OBBOBB(obb1, obb2): # no separating axis found, # the two boxes overlap return True - - - def StateSpace(env, factor=0): boundary = env.boundary @@ -242,10 +264,13 @@ def isCollide(initparams, x, child, dist): if lineAABB(x, child, dist, initparams.env.AABB[i]): return True, dist for i in initparams.env.balls: - # if isinball(i, child): + # if isinball(i, child): i # return True, dist if lineSphere(x, child, i): return True, dist + for i in initparams.env.OBB: + if lineOBB(x, child, dist, i): + return True, dist return False, dist @@ -256,6 +281,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 +330,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))