update

LICENSE

@@ -1 +1,21 @@
-Copyright (c) 2020 Huiming Zhou & Yue Qi
+MIT License
+
+Copyright (c) 2020 Huiming Zhou
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

Sampling_based_Planning/rrt_2D/plotting.py

@@ -1,5 +1,5 @@
 """
-Plotting tools for RRT_2D
+Plotting tools for Sampling-based algorithms
 @author: huiming zhou
 """
 

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

Sampling_based_Planning/rrt_3D/extend_rrt3D.py

@@ -58,7 +58,7 @@ class extend_rrt(object):
             
         # return rrt_tree
         self.done = True
-        self.Path, D = path(self)
+        self.Path, _ = path(self)
         visualization(self)
         plt.show()
         

Sampling_based_Planning/rrt_3D/plot_util3D.py

@@ -90,7 +90,6 @@ def visualization(initparams):
         # V = np.array(list(initparams.V))
         # E = initparams.E
         #----------- end
-        V = np.array(initparams.V)
         # edges = initparams.E
         Path = np.array(initparams.Path)
         start = initparams.env.start

Search_based_Planning/Search_2D/Astar.py

@@ -11,10 +11,12 @@ import heapq
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search_based_Planning/")
 
-from Search_based_Planning.Search_2D import plotting, env
+from Search_2D import plotting, env
 
 
 class AStar:
+    """AStar set the cost + heuristics as the priority
+    """
     def __init__(self, s_start, s_goal, heuristic_type):
         self.s_start = s_start
         self.s_goal = s_goal

Search_based_Planning/Search_2D/Best_First.py

@@ -11,116 +11,53 @@ import heapq
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search_based_Planning/")
 
-from Search_based_Planning.Search_2D import plotting, env
+from Search_2D import plotting, env
+from Search_2D.Astar import AStar
 
 
-class BestFirst:
-    def __init__(self, s_start, s_goal):
-        self.s_start = s_start
-        self.s_goal = s_goal
-
-        self.Env = env.Env()
-        self.plotting = plotting.Plotting(self.s_start, self.s_goal)
-
-        self.u_set = self.Env.motions  # feasible input set
-        self.obs = self.Env.obs  # position of obstacles
-
-        self.OPEN = []  # OPEN set: visited nodes
-        self.CLOSED = []  # CLOSED set / visited order
-        self.PARENT = dict()  # recorded parent
-
+class BestFirst(AStar):
+    """BestFirst set the heuristics as the priority 
+    """
     def searching(self):
         """
-        Best-first Searching
-        :return: planning path, visited order
+        Breadth-first Searching.
+        :return: path, visited order
         """
 
         self.PARENT[self.s_start] = self.s_start
+        self.g[self.s_start] = 0
+        self.g[self.s_goal] = math.inf
         heapq.heappush(self.OPEN,
                        (self.heuristic(self.s_start), self.s_start))
 
         while self.OPEN:
             _, s = heapq.heappop(self.OPEN)
+            self.CLOSED.append(s)
 
             if s == self.s_goal:
                 break
-            self.CLOSED.append(s)
 
             for s_n in self.get_neighbor(s):
-                if self.is_collision(s, s_n):
-                    continue
-
-                if s_n not in self.PARENT:  # node not explored
-                    heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n))
-                    self.PARENT[s_n] = s
-
-        return self.extract_path(), self.CLOSED
-
-    def heuristic(self, s):
-        """
-        estimated distance between current state and goal state.
-        :param s: current state
-        :return: Euclidean distance
-        """
-
-        return math.hypot(s[0] - self.s_goal[0], s[1] - self.s_goal[1])
-
-    def get_neighbor(self, s):
-        """
-        find neighbors of state s that not in obstacles.
-        :param s: state
-        :return: neighbors
-        """
-
-        return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]
-
-    def is_collision(self, s_start, s_end):
-        """
-        check if the line segment (s_start, s_end) is collision.
-        :param s_start: start node
-        :param s_end: end node
-        :return: True: is collision / False: not collision
-        """
-
-        if s_start in self.obs or s_end in self.obs:
-            return True
-
-        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
-            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
-                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
-                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
-            else:
-                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
-                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+                new_cost = self.g[s] + self.cost(s, s_n)
 
-            if s1 in self.obs or s2 in self.obs:
-                return True
+                if s_n not in self.g:
+                    self.g[s_n] = math.inf
 
-        return False
-
-    def extract_path(self):
-        """
-        Extract the path based on the relationship of nodes.
-        :return: The planning path
-        """
-
-        path = [self.s_goal]
-        s = self.s_goal
+                if new_cost < self.g[s_n]:  # conditions for updating Cost
+                    self.g[s_n] = new_cost
+                    self.PARENT[s_n] = s
 
-        while True:
-            s = self.PARENT[s]
-            path.append(s)
-            if s == self.s_start:
-                break
+                    # best first set the heuristics as the priority 
+                    heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n))
 
-        return list(path)
+        return self.extract_path(self.PARENT), self.CLOSED
 
 
 def main():
     s_start = (5, 5)
     s_goal = (45, 25)
 
-    BF = BestFirst(s_start, s_goal)
+    BF = BestFirst(s_start, s_goal, 'euclidean')
     plot = plotting.Plotting(s_start, s_goal)
 
     path, visited = BF.searching()

Search_based_Planning/Search_2D/Dijkstra.py

@@ -11,35 +11,25 @@ import heapq
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search_based_Planning/")
 
-from Search_based_Planning.Search_2D import plotting, env
+from Search_2D import plotting, env
 
+from Search_2D.Astar import AStar
 
-class Dijkstra:
-    def __init__(self, s_start, s_goal):
-        self.s_start = s_start
-        self.s_goal = s_goal
-
-        self.Env = env.Env()
-        self.plotting = plotting.Plotting(self.s_start, self.s_goal)
-
-        self.u_set = self.Env.motions  # feasible input set
-        self.obs = self.Env.obs  # position of obstacles
-
-        self.OPEN = []  # priority queue / OPEN set
-        self.CLOSED = []  # closed set & visited
-        self.PARENT = dict()  # record parent
-        self.g = dict()  # Cost to come
 
+class Dijkstra(AStar):
+    """Dijkstra set the cost as the priority 
+    """
     def searching(self):
         """
-        Dijkstra Searching.
+        Breadth-first Searching.
         :return: path, visited order
         """
 
         self.PARENT[self.s_start] = self.s_start
         self.g[self.s_start] = 0
         self.g[self.s_goal] = math.inf
-        heapq.heappush(self.OPEN, (0, self.s_start))
+        heapq.heappush(self.OPEN,
+                       (0, self.s_start))
 
         while self.OPEN:
             _, s = heapq.heappop(self.OPEN)
@@ -50,85 +40,25 @@ class Dijkstra:
 
             for s_n in self.get_neighbor(s):
                 new_cost = self.g[s] + self.cost(s, s_n)
+
                 if s_n not in self.g:
                     self.g[s_n] = math.inf
-                if new_cost < self.g[s_n]:
+
+                if new_cost < self.g[s_n]:  # conditions for updating Cost
                     self.g[s_n] = new_cost
-                    heapq.heappush(self.OPEN, (new_cost, s_n))
                     self.PARENT[s_n] = s
 
-        return self.extract_path(), self.CLOSED
-
-    def get_neighbor(self, s):
-        """
-        find neighbors of state s that not in obstacles.
-        :param s: state
-        :return: neighbors
-        """
-
-        return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]
-
-    def extract_path(self):
-        """
-        Extract the path based on PARENT set.
-        :return: The planning path
-        """
-
-        path = [self.s_goal]
-        s = self.s_goal
-
-        while True:
-            s = self.PARENT[s]
-            path.append(s)
-
-            if s == self.s_start:
-                break
-
-        return list(path)
-
-    def cost(self, s_start, s_goal):
-        """
-        Calculate Cost for this motion
-        :param s_start: starting node
-        :param s_goal: end node
-        :return:  Cost for this motion
-        """
-
-        if self.is_collision(s_start, s_goal):
-            return math.inf
-
-        return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
-
-    def is_collision(self, s_start, s_end):
-        """
-        check if the line segment (s_start, s_end) is collision.
-        :param s_start: start node
-        :param s_end: end node
-        :return: True: is collision / False: not collision
-        """
-
-        if s_start in self.obs or s_end in self.obs:
-            return True
-
-        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
-            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
-                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
-                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
-            else:
-                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
-                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
-
-            if s1 in self.obs or s2 in self.obs:
-                return True
+                    # best first set the heuristics as the priority 
+                    heapq.heappush(self.OPEN, (new_cost, s_n))
 
-        return False
+        return self.extract_path(self.PARENT), self.CLOSED
 
 
 def main():
     s_start = (5, 5)
     s_goal = (45, 25)
 
-    dijkstra = Dijkstra(s_start, s_goal)
+    dijkstra = Dijkstra(s_start, s_goal, 'None')
     plot = plotting.Plotting(s_start, s_goal)
 
     path, visited = dijkstra.searching()

Search_based_Planning/Search_2D/bfs.py

@@ -10,24 +10,14 @@ from collections import deque
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search_based_Planning/")
 
-from Search_based_Planning.Search_2D import plotting, env
-
-
-class BFS:
-    def __init__(self, s_start, s_goal):
-        self.s_start = s_start
-        self.s_goal = s_goal
-
-        self.Env = env.Env()
-        self.plotting = plotting.Plotting(self.s_start, self.s_goal)
-
-        self.u_set = self.Env.motions  # feasible input set
-        self.obs = self.Env.obs  # position of obstacles
-
-        self.OPEN = deque()  # OPEN set: visited nodes
-        self.PARENT = dict()  # recorded parent
-        self.CLOSED = []  # CLOSED set: explored nodes
-
+from Search_2D import plotting, env
+from Search_2D.Astar import AStar
+import math
+import heapq
+
+class BFS(AStar):
+    """BFS add the new visited node in the end of the openset
+    """
     def searching(self):
         """
         Breadth-first Searching.
@@ -35,81 +25,40 @@ class BFS:
         """
 
         self.PARENT[self.s_start] = self.s_start
-        self.OPEN.append(self.s_start)
+        self.g[self.s_start] = 0
+        self.g[self.s_goal] = math.inf
+        heapq.heappush(self.OPEN,
+                       (0, self.s_start))
 
         while self.OPEN:
-            s = self.OPEN.popleft()
+            _, s = heapq.heappop(self.OPEN)
+            self.CLOSED.append(s)
 
             if s == self.s_goal:
                 break
-            self.CLOSED.append(s)
 
             for s_n in self.get_neighbor(s):
-                if self.is_collision(s, s_n):
-                    continue
-                if s_n not in self.PARENT:  # node not explored
-                    self.OPEN.append(s_n)
-                    self.PARENT[s_n] = s
-
-        return self.extract_path(), self.CLOSED
-
-    def get_neighbor(self, s):
-        """
-        find neighbors of state s that not in obstacles.
-        :param s: state
-        :return: neighbors : [nodes]
-        """
+                new_cost = self.g[s] + self.cost(s, s_n)
 
-        return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]
+                if s_n not in self.g:
+                    self.g[s_n] = math.inf
 
-    def is_collision(self, s_start, s_end):
-        """
-        check if the line segment (s_start, s_end) is collision.
-        :param s_start: start node
-        :param s_end: end node
-        :return: True: is collision / False: not collision
-        """
-
-        if s_start in self.obs or s_end in self.obs:
-            return True
-
-        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
-            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
-                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
-                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
-            else:
-                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
-                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
-
-            if s1 in self.obs or s2 in self.obs:
-                return True
-
-        return False
-
-    def extract_path(self):
-        """
-        Extract the path based on the PARENT set.
-        :return: The planning path : [nodes]
-        """
-
-        path = [self.s_goal]
-        s = self.s_goal
-
-        while True:
-            s = self.PARENT[s]
-            path.append(s)
+                if new_cost < self.g[s_n]:  # conditions for updating Cost
+                    self.g[s_n] = new_cost
+                    self.PARENT[s_n] = s
 
-            if s == self.s_start:
-                break
+                    # bfs, add new node to the end of the openset
+                    prior = self.OPEN[-1][0]+1 if len(self.OPEN)>0 else 0
+                    heapq.heappush(self.OPEN, (prior, s_n))
 
-        return list(path)
+        return self.extract_path(self.PARENT), self.CLOSED
 
 
 def main():
     s_start = (5, 5)
     s_goal = (45, 25)
 
-    bfs = BFS(s_start, s_goal)
+    bfs = BFS(s_start, s_goal, 'None')
     plot = plotting.Plotting(s_start, s_goal)
 
     path, visited = bfs.searching()

Search_based_Planning/Search_2D/dfs.py

@@ -1,117 +1,63 @@
-"""
-Depth-first Searching_2D (DFS)
-@author: huiming zhou
-"""
 
 import os
 import sys
-from collections import deque
+import math
+import heapq
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search_based_Planning/")
 
-from Search_based_Planning.Search_2D import plotting, env
-
-
-class DFS:
-    def __init__(self, s_start, s_goal):
-        self.s_start = s_start
-        self.s_goal = s_goal
-
-        self.Env = env.Env()
-        self.plotting = plotting.Plotting(self.s_start, self.s_goal)
-
-        self.u_set = self.Env.motions  # feasible input set
-        self.obs = self.Env.obs  # position of obstacles
-
-        self.OPEN = deque()  # OPEN set: visited nodes
-        self.PARENT = dict()  # recorded parent
-        self.CLOSED = []  # CLOSED set / visited order
+from Search_2D import plotting, env
+from Search_2D.Astar import AStar
 
+class DFS(AStar):
+    """DFS add the new visited node in the front of the openset
+    """
     def searching(self):
         """
-        Depth-first Searching
-        :return: planning path, visited order
+        Breadth-first Searching.
+        :return: path, visited order
         """
 
         self.PARENT[self.s_start] = self.s_start
-        self.OPEN.append(self.s_start)
+        self.g[self.s_start] = 0
+        self.g[self.s_goal] = math.inf
+        heapq.heappush(self.OPEN,
+                       (0, self.s_start))
 
         while self.OPEN:
-            s = self.OPEN.pop()
+            _, s = heapq.heappop(self.OPEN)
+            self.CLOSED.append(s)
 
             if s == self.s_goal:
                 break
-            self.CLOSED.append(s)
 
             for s_n in self.get_neighbor(s):
-                if self.is_collision(s, s_n):
-                    continue
-                if s_n not in self.PARENT:  # node not explored
-                    self.OPEN.append(s_n)
-                    self.PARENT[s_n] = s
-
-        return self.extract_path(), self.CLOSED
-
-    def get_neighbor(self, s):
-        """
-        find neighbors of state s that not in obstacles.
-        :param s: state
-        :return: neighbors : [nodes]
-        """
+                new_cost = self.g[s] + self.cost(s, s_n)
 
-        return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]
+                if s_n not in self.g:
+                    self.g[s_n] = math.inf
 
-    def is_collision(self, s_start, s_end):
-        """
-        check if the line segment (s_start, s_end) is collision.
-        :param s_start: start node
-        :param s_end: end node
-        :return: True: is collision / False: not collision
-        """
-
-        if s_start in self.obs or s_end in self.obs:
-            return True
-
-        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
-            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
-                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
-                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
-            else:
-                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
-                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
-
-            if s1 in self.obs or s2 in self.obs:
-                return True
-
-        return False
-
-    def extract_path(self):
-        """
-        Extract the path based on the relationship of nodes.
-        :return: The planning path
-        """
-
-        path = [self.s_goal]
-        s = self.s_goal
+                if new_cost < self.g[s_n]:  # conditions for updating Cost
+                    self.g[s_n] = new_cost
+                    self.PARENT[s_n] = s
 
-        while True:
-            s = self.PARENT[s]
-            path.append(s)
-            if s == self.s_start:
-                break
+                    # dfs, add new node to the front of the openset
+                    prior = self.OPEN[0][0]-1 if len(self.OPEN)>0 else 0
+                    heapq.heappush(self.OPEN, (prior, s_n))
 
-        return list(path)
+        return self.extract_path(self.PARENT), self.CLOSED
 
 
 def main():
     s_start = (5, 5)
     s_goal = (45, 25)
 
-    dfs = DFS(s_start, s_goal)
+    dfs = DFS(s_start, s_goal, 'None')
     plot = plotting.Plotting(s_start, s_goal)
 
     path, visited = dfs.searching()
+    visited = list(dict.fromkeys(visited))
     plot.animation(path, visited, "Depth-first Searching (DFS)")  # animation
 
 

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: