diff --git a/Sampling_based_Planning/rrt_3D/BIT_star3D.py b/Sampling_based_Planning/rrt_3D/BIT_star3D.py index fb1a551..5a1617e 100644 --- a/Sampling_based_Planning/rrt_3D/BIT_star3D.py +++ b/Sampling_based_Planning/rrt_3D/BIT_star3D.py @@ -53,14 +53,14 @@ class BIT_star: self.env = env() self.xstart, self.xgoal = tuple(self.env.start), tuple(self.env.goal) self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) - self.maxiter = 5000 # used for determining how many batches needed + self.maxiter = 1000 # used for determining how many batches needed # radius calc parameter: # larger value makes better 1-time-performance, but longer time trade off self.eta = 7 # bigger or equal to 1 # sampling - self.m = 1000 # number of samples for one time sample + self.m = 400 # number of samples for one time sample self.d = 3 # dimension we work with # instance of the cost to come gT diff --git a/Sampling_based_Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc index e233e09..5c14f7b 100644 Binary files a/Sampling_based_Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc and b/Sampling_based_Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc differ diff --git a/Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc index 93843d0..4ffcb79 100644 Binary files a/Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc and b/Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc differ diff --git a/Sampling_based_Planning/rrt_3D/__pycache__/queue.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/queue.cpython-37.pyc index d7f5ebe..895cc98 100644 Binary files a/Sampling_based_Planning/rrt_3D/__pycache__/queue.cpython-37.pyc and b/Sampling_based_Planning/rrt_3D/__pycache__/queue.cpython-37.pyc differ diff --git a/Sampling_based_Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc index ec095f0..70b69f3 100644 Binary files a/Sampling_based_Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc and b/Sampling_based_Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc differ diff --git a/Sampling_based_Planning/rrt_3D/env3D.py b/Sampling_based_Planning/rrt_3D/env3D.py index 43fc509..df36ad0 100644 --- a/Sampling_based_Planning/rrt_3D/env3D.py +++ b/Sampling_based_Planning/rrt_3D/env3D.py @@ -132,6 +132,11 @@ class env(): # theta stands for rotational angles around three principle axis in world frame # translation stands for translation in the world frame ori = [self.OBB[obb_to_move]] + # move obb position + self.OBB[obb_to_move].P = \ + [self.OBB[obb_to_move].P[0] + translation[0], + self.OBB[obb_to_move].P[1] + translation[1], + self.OBB[obb_to_move].P[2] + translation[2]] # Calculate orientation self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2]) # generating transformation matrix diff --git a/Search_based_Planning/Search_3D/Anytime_Dstar3D.py b/Search_based_Planning/Search_3D/Anytime_Dstar3D.py index bc7fa62..cf84bd7 100644 --- a/Search_based_Planning/Search_3D/Anytime_Dstar3D.py +++ b/Search_based_Planning/Search_3D/Anytime_Dstar3D.py @@ -9,7 +9,7 @@ from collections import defaultdict sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env -from Search_3D.utils3D import getDist, heuristic_fun, getNearest, isinbound, \ +from Search_3D.utils3D import getDist, heuristic_fun, getNearest, isinbound, isinobb, \ cost, children, StateSpace from Search_3D.plot_util3D import visualization from Search_3D import queue @@ -91,7 +91,10 @@ class Anytime_Dstar(object): # scan graph for changed Cost, if Cost is changed update it CHANGED = set() for xi in self.CLOSED: - if isinbound(old, xi, mode) or isinbound(new, xi, mode): + if self.isinobs(old, xi, mode) or self.isinobs(new, xi, mode): + # if self.isinobs(new, xi, mode): + self.V.remove(xi) + # self.V.difference_update({i for i in children(self, xi)}) newchildren = set(children(self, xi)) # B self.CHILDREN[xi] = newchildren for xj in newchildren: @@ -99,6 +102,12 @@ class Anytime_Dstar(object): CHANGED.add(xi) return CHANGED + def isinobs(self, obs, x, mode): + if mode == 'obb': + return isinobb(obs, x) + elif mode == 'aabb': + return isinbound(obs, x, mode) + # def updateGraphCost(self, range_changed=None, new=None, old=None, mode=False): # # TODO scan graph for changed Cost, if Cost is changed update it # # make the graph Cost via vectorization @@ -172,7 +181,9 @@ class Anytime_Dstar(object): break # change environment # 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') + # new2, old2 = self.env.move_block(a=[0, 0, -0.2], mode='translation') + new2, old2 = self.env.move_OBB(theta=[10*t, 0, 0], translation=[0, 0.1*t, 0]) + mmode = 'obb' # obb or aabb ischanged = True # islargelychanged = True self.Path = [] @@ -180,7 +191,7 @@ class Anytime_Dstar(object): # update Cost with changed environment if ischanged: # CHANGED = self.updatecost(True, new2, old2, mode='obb') - CHANGED = self.updatecost(True, new2, old2) + CHANGED = self.updatecost(True, new2, old2, mode=mmode) for u in CHANGED: self.UpdateState(u) self.ComputeorImprovePath() diff --git a/Search_based_Planning/Search_3D/Astar3D.py b/Search_based_Planning/Search_3D/Astar3D.py index 4b7ad68..066c4b0 100644 --- a/Search_based_Planning/Search_3D/Astar3D.py +++ b/Search_based_Planning/Search_3D/Astar3D.py @@ -29,7 +29,7 @@ class Weighted_A_star(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), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)} - self.settings = 'CollisionChecking' + self.settings = 'NonCollisionChecking' # 'NonCollisionChecking' or 'CollisionChecking' self.env = env(resolution=resolution) self.start, self.goal = tuple(self.env.start), tuple(self.env.goal) self.g = {self.start:0,self.goal:np.inf} @@ -65,12 +65,8 @@ class Weighted_A_star(object): if a < self.g[xj]: self.g[xj] = a self.Parent[xj] = xi - # if (a, xj) in self.OPEN.enumerate(): - # update priority of xj + # assign or update the priority in the open self.OPEN.put(xj, a + 1 * heuristic_fun(self, xj)) - # else: - # add xj in to OPEN set - # self.OPEN.put(xj, a + 1 * heuristic_fun(self, xj)) # For specified expanded nodes, used primarily in LRTA* if N: if len(self.CLOSED) % N == 0: @@ -84,7 +80,7 @@ class Weighted_A_star(object): self.done = True self.Path = self.path() if N is None: - #visualization(self) + visualization(self) plt.show() return True diff --git a/Search_based_Planning/Search_3D/Dstar3D.py b/Search_based_Planning/Search_3D/Dstar3D.py index 523d462..e0a69ce 100644 --- a/Search_based_Planning/Search_3D/Dstar3D.py +++ b/Search_based_Planning/Search_3D/Dstar3D.py @@ -24,6 +24,7 @@ class D_star(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), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)} + self.settings = 'CollisionChecking' 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) @@ -165,8 +166,8 @@ class D_star(object): # when the environemnt changes over time for i in range(5): - self.env.move_block(a=[0.1, 0, 0], s=0.5, block_to_move=1, mode='translation') - self.env.move_block(a=[0, 0, -0.25], s=0.5, block_to_move=0, mode='translation') + self.env.move_block(a=[0, -0.50, 0], s=0.5, block_to_move=1, mode='translation') + self.env.move_block(a=[-0.25, 0, 0], s=0.5, block_to_move=0, mode='translation') # travel from end to start s = tuple(self.env.start) # self.V = set() diff --git a/Search_based_Planning/Search_3D/DstarLite3D.py b/Search_based_Planning/Search_3D/DstarLite3D.py index 316de8f..87d7031 100644 --- a/Search_based_Planning/Search_3D/DstarLite3D.py +++ b/Search_based_Planning/Search_3D/DstarLite3D.py @@ -151,7 +151,7 @@ class D_star_Lite(object): 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_OBB(theta = [0,0.1*t,0]) #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 = [] diff --git a/Search_based_Planning/Search_3D/LP_Astar3D.py b/Search_based_Planning/Search_3D/LP_Astar3D.py index 7a5ca1d..cacb6d6 100644 --- a/Search_based_Planning/Search_3D/LP_Astar3D.py +++ b/Search_based_Planning/Search_3D/LP_Astar3D.py @@ -8,7 +8,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_base from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isinbound, isinball, \ - cost, obstacleFree + cost, obstacleFree, isCollide from Search_3D.plot_util3D import visualization import queue import pyrr @@ -110,7 +110,7 @@ class Lifelong_Astar(object): return False, dist def cost(self, x, y): - collide, dist = self.isCollide(x, y) + collide, dist = isCollide(self, x, y) if collide: return np.inf else: return dist @@ -128,7 +128,7 @@ class Lifelong_Astar(object): gset = [self.g[xi] for xi in nei] # collision check and make g Cost inf for i in range(len(nei)): - if self.isCollide(nei[i],j)[0]: + if isCollide(self, nei[i],j)[0]: gset[i] = np.inf parent = nei[np.argmin(gset)] path.append([x, parent]) @@ -168,10 +168,10 @@ class Lifelong_Astar(object): self.Path = self.path() self.done = True visualization(self) - plt.pause(2) + plt.pause(1) def change_env(self): - self.env.New_block() + _, _ = self.env.move_block(block_to_move=1,a = [0,2,0]) self.done = False self.Path = [] self.CLOSED = set() @@ -182,10 +182,10 @@ class Lifelong_Astar(object): if __name__ == '__main__': sta = time.time() Astar = Lifelong_Astar(1) - Astar.ComputePath() - Astar.change_env() - Astar.ComputePath() - plt.show() + for i in range(5): + Astar.change_env() + Astar.ComputePath() + plt.pause(1) print(time.time() - sta) \ No newline at end of file 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 748cceb..38d9cd4 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 01f0334..66567ea 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 63be0ba..477a39b 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__/queue.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/queue.cpython-37.pyc index 4c7b1cd..ad7eef7 100644 Binary files a/Search_based_Planning/Search_3D/__pycache__/queue.cpython-37.pyc and b/Search_based_Planning/Search_3D/__pycache__/queue.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 8a2759e..86b81ce 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/bidirectional_Astar3D.py b/Search_based_Planning/Search_3D/bidirectional_Astar3D.py index 2f1c48d..c849aa8 100644 --- a/Search_based_Planning/Search_3D/bidirectional_Astar3D.py +++ b/Search_based_Planning/Search_3D/bidirectional_Astar3D.py @@ -30,6 +30,7 @@ class Weighted_A_star(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.settings = 'NonCollisionChecking' self.start, self.goal = tuple(self.env.start), tuple(self.env.goal) self.g = {self.start:0,self.goal:0} self.OPEN1 = queue.MinheapPQ() # store [point,priority] diff --git a/Search_based_Planning/Search_3D/env3D.py b/Search_based_Planning/Search_3D/env3D.py index 58090a2..c806ff4 100644 --- a/Search_based_Planning/Search_3D/env3D.py +++ b/Search_based_Planning/Search_3D/env3D.py @@ -1,15 +1,13 @@ -# this is the three dimensional configuration space for rrt +# this is the three dimensional space # !/usr/bin/env python3 # -*- coding: utf-8 -*- """ @author: yue qi """ import numpy as np - - # from utils3D import OBB2AABB -def R_matrix(z_angle, y_angle, x_angle): +def R_matrix(z_angle,y_angle,x_angle): # s angle: row; y angle: pitch; z angle: yaw # generate rotation matrix in SO3 # RzRyRx = R, ZYX intrinsic rotation @@ -17,29 +15,29 @@ def R_matrix(z_angle, y_angle, x_angle): # 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)]]) - + 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 - 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], - # [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00], - # [1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00], - # [6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00], - [1.21e+01, 0.00e+00, 0.00e+00, 1.29e+01, 5.00e+00, 3.90e+00], - [1.81e+01, 0.00e+00, 0.00e+00, 1.89e+01, 5.00e+00, 3.90e+00]] + block = [[4.00e+00, 1.20e+01, 0.00e+00, 5.00e+00, 2.00e+01, 5.00e+00], + [5.5e+00, 1.20e+01, 0.00e+00, 1.00e+01, 1.30e+01, 5.00e+00], + [1.00e+01, 1.20e+01, 0.00e+00, 1.40e+01, 1.30e+01, 5.00e+00], + [1.00e+01, 9.00e+00, 0.00e+00, 2.00e+01, 1.00e+01, 5.00e+00], + [9.00e+00, 6.00e+00, 0.00e+00, 1.00e+01, 1.00e+01, 5.00e+00]] Obstacles = [] for i in block: i = np.array(i) Obstacles.append([j for j in i]) return np.array(Obstacles) +def getballs(): + spheres = [[2.0,6.0,2.5,1.0],[14.0,14.0,2.5,2]] + Obstacles = [] + for i in spheres: + Obstacles.append([j for j in i]) + return np.array(Obstacles) def getAABB(blocks): # used for Pyrr package for detecting collision @@ -48,17 +46,25 @@ def getAABB(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 +def getAABB2(blocks): + # used in lineAABB + AABB = [] + for i in blocks: + AABB.append(aabb(i)) + return AABB + +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 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]] - + 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 @@ -68,112 +74,74 @@ 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 - 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 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 - + self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]]) class env(): - def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1): + def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=20, zmax=5, resolution=1): + # def __init__(self, xmin=-5, ymin=0, zmin=-5, xmax=10, ymax=5, zmax=10, resolution=1): self.resolution = resolution - self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) + 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 + self.OBB = np.array([obb([5.0,7.0,2.5],[0.5,2.0,2.5],R_matrix(135,0,0)), + obb([12.0,4.0,2.5],[0.5,2.0,2.5],R_matrix(45,0,0))]) + self.start = np.array([2.0, 2.0, 2.0]) + self.goal = np.array([6.0, 16.0, 0.0]) + self.t = 0 # time def New_block(self): newblock = add_block() - self.blocks = np.vstack([self.blocks, newblock]) + 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'): + def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], block_to_move = 0, mode = 'translation'): # 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 # (s',t') = (s + 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 # (s',t') = (s + 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]]) + 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]]) 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.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 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 # (s',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] - - + def move_OBB(self, obb_to_move = 0, theta=[0,0,0], translation=[0,0,0]): + # theta stands for rotational angles around three principle axis in world frame + # translation stands for translation in the world frame + ori = [self.OBB[obb_to_move]] + self.OBB[obb_to_move].P = \ + [self.OBB[obb_to_move].P[0] + translation[0], + self.OBB[obb_to_move].P[1] + translation[1], + self.OBB[obb_to_move].P[2] + translation[2]] + # Calculate orientation + self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2]) + # generating transformation matrix + 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]),[translation[0],translation[1],translation[2],1]]) + return self.OBB[obb_to_move], ori[0] + if __name__ == '__main__': - newenv = env() + newenv = env() \ No newline at end of file diff --git a/Search_based_Planning/Search_3D/plot_util3D.py b/Search_based_Planning/Search_3D/plot_util3D.py index afea609..5cf5a5a 100644 --- a/Search_based_Planning/Search_3D/plot_util3D.py +++ b/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=120.) + ax.view_init(elev=90., azim=0.) #ax.view_init(elev=-8., azim=180) ax.clear() # drawing objects @@ -107,68 +107,49 @@ def visualization(initparams): 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 - xmin, xmax = initparams.env.boundary[0], initparams.env.boundary[3] - ymin, ymax = initparams.env.boundary[1], initparams.env.boundary[4] - zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5] - dx, dy, dz = xmax-xmin, ymax-ymin, zmax-zmin - ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy) - plt.xlabel('s') - plt.ylabel('y') + set_axes_equal(ax) + make_transparent(ax) + # plt.xlabel('s') + # plt.ylabel('y') plt.pause(0.0001) -def make_get_proj(self, rx, ry, rz): - ''' - Return a variation on :func:`~mpl_toolkit.mplot2d.axes3d.Axes3D.getproj` that - makes the box aspect ratio equal to *rx:ry:rz*, using an axes object *self*. +def set_axes_equal(ax): + '''Make axes of 3D plot have equal scale so that spheres appear as spheres, + cubes as cubes, etc.. This is one possible solution to Matplotlib's + ax.set_aspect('equal') and ax.axis('equal') not working for 3D. + https://stackoverflow.com/questions/13685386/matplotlib-equal-unit-length-with-equal-aspect-ratio-z-axis-is-not-equal-to + Input + ax: a matplotlib axis, e.g., as output from plt.gca(). ''' - rm = max(rx, ry, rz) - kx = rm / rx; ky = rm / ry; kz = rm / rz + x_limits = ax.get_xlim3d() + y_limits = ax.get_ylim3d() + z_limits = ax.get_zlim3d() - # Copied directly from mpl_toolkit/mplot3d/axes3d.py. New or modified lines are - # marked by ## - def get_proj(): - relev, razim = np.pi * self.elev/180, np.pi * self.azim/180 + x_range = abs(x_limits[1] - x_limits[0]) + x_middle = np.mean(x_limits) + y_range = abs(y_limits[1] - y_limits[0]) + y_middle = np.mean(y_limits) + z_range = abs(z_limits[1] - z_limits[0]) + z_middle = np.mean(z_limits) - xmin, xmax = self.get_xlim3d() - ymin, ymax = self.get_ylim3d() - zmin, zmax = self.get_zlim3d() + # The plot bounding box is a sphere in the sense of the infinity + # norm, hence I call half the max range the plot radius. + plot_radius = 0.5*max([x_range, y_range, z_range]) - # transform to uniform world coordinates 0-1.0,0-1.0,0-1.0 - worldM = proj3d.world_transformation(xmin, xmax, - ymin, ymax, - zmin, zmax) - ratio = 0.5 - # adjust the aspect ratio ## - aspectM = proj3d.world_transformation(-kx + 1, kx, ## - -ky + 1, ky, ## - -kz + 1, kz) ## + ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius]) + ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius]) + ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius]) - # look into the middle of the new coordinates - R = np.array([0.5, 0.5, 0.5]) - - xp = R[0] + np.cos(razim) * np.cos(relev) * self.dist *ratio - yp = R[1] + np.sin(razim) * np.cos(relev) * self.dist *ratio - zp = R[2] + np.sin(relev) * self.dist *ratio - E = np.array((xp, yp, zp)) - - self.eye = E - self.vvec = R - E - self.vvec = self.vvec / np.linalg.norm(self.vvec) - - if abs(relev) > np.pi/2: - # upside down - V = np.array((0, 0, -1)) - else: - V = np.array((0, 0, 1)) - zfront, zback = -self.dist *ratio, self.dist *ratio - - viewM = proj3d.view_transformation(E, R, V) - perspM = proj3d.persp_transformation(zfront, zback) - M0 = np.dot(viewM, np.dot(aspectM, worldM)) ## - M = np.dot(perspM, M0) - return M - return get_proj +def make_transparent(ax): + # make the panes transparent + ax.xaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) + ax.yaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) + ax.zaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) + # make the grid lines transparent + ax.xaxis._axinfo["grid"]['color'] = (1,1,1,0) + ax.yaxis._axinfo["grid"]['color'] = (1,1,1,0) + ax.zaxis._axinfo["grid"]['color'] = (1,1,1,0) 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 69cdd8d..5018be7 100644 --- a/Search_based_Planning/Search_3D/utils3D.py +++ b/Search_based_Planning/Search_3D/utils3D.py @@ -262,7 +262,7 @@ def g_Space(initparams): g[v] = np.inf # this hashmap initialize all g values at inf return g -def isCollide(initparams, x, child, dist): +def isCollide(initparams, x, child, dist=None): '''see if line intersects obstacle''' '''specified for expansion in A* 3D lookup table''' if dist==None: