search_update

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

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

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()

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
 

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()

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 = []

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)

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]

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()

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
-
-    # 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
-
-        xmin, xmax = self.get_xlim3d()
-        ymin, ymax = self.get_ylim3d()
-        zmin, zmax = self.get_zlim3d()
-
-        # 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) ##
-
-        # 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
+    x_limits = ax.get_xlim3d()
+    y_limits = ax.get_ylim3d()
+    z_limits = ax.get_zlim3d()
+
+    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)
+
+    # 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])
+
+    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])
+
+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

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: