update format

Sampling-based Planning/.idea/.gitignore

@@ -0,0 +1,3 @@
+# Default ignored files
+/shelf/
+/workspace.xml

Sampling-based Planning/.idea/Sampling-based Planning.iml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="jdk" jdkName="Python 3.7 (Search-based Planning)" jdkType="Python SDK" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>

Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

Sampling-based Planning/.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7 (Search-based Planning)" project-jdk-type="Python SDK" />
+</project>

Sampling-based Planning/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/Sampling-based Planning.iml" filepath="$PROJECT_DIR$/.idea/Sampling-based Planning.iml" />
+    </modules>
+  </component>
+</project>

Sampling-based Planning/.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+  </component>
+</project>

Sampling-based Planning/rrt_2D/env.py

@@ -0,0 +1,45 @@
+class Env:
+    def __init__(self):
+        self.x_range = (0, 50)
+        self.y_range = (0, 30)
+        self.obs_boundary = self.obs_boundary()
+        self.obs_circle = self.obs_circle()
+        self.obs_rectangle = self.obs_rectangle()
+
+    @staticmethod
+    def obs_boundary():
+        obs_boundary = [
+            (0, 0, 1, 30),
+            (0, 30, 50, 1),
+            (1, 0, 50, 1),
+            (50, 1, 1, 30)
+            # (20, 1, 1, 15),
+            # (10, 15, 10, 1),
+            # (30, 15, 1, 15),
+            # (40, 1, 1, 15)
+        ]
+        return obs_boundary
+
+    @staticmethod
+    def obs_rectangle():
+        obs_rectangle = [
+            (13, 10, 5, 3),
+            (18, 4, 5, 4),
+            (22, 13, 6, 3),
+            (33, 15, 5, 3),
+            (42, 6, 5, 3)
+        ]
+        return obs_rectangle
+
+    @staticmethod
+    def obs_circle():
+        obs_cir = [
+            (5, 10, 3),
+            (10, 22, 3.5),
+            (21, 23, 3),
+            (34, 9, 4),
+            (37, 23, 3),
+            (45, 20, 2)
+        ]
+
+        return obs_cir

Sampling-based Planning/rrt_2D/plotting.py

@@ -0,0 +1,80 @@
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+
+from rrt_2D import env
+
+
+class Plotting:
+    def __init__(self, xI, xG):
+        self.xI, self.xG = xI, xG
+        self.env = env.Env()
+        self.obs_bound = self.env.obs_boundary
+        self.obs_circle = self.env.obs_circle
+        self.obs_rectangle = self.env.obs_rectangle
+
+    def animation(self, nodelist, path, animation=False):
+        if path is None:
+            print("No path found!")
+            return
+        self.plot_grid("RRT")
+        self.plot_visited(nodelist, animation)
+        self.plot_path(path)
+
+    def plot_grid(self, name):
+        fig, ax = plt.subplots()
+
+        for (ox, oy, w, h) in self.obs_bound:
+            ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='black',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, w, h) in self.obs_rectangle:
+            ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, r) in self.obs_circle:
+            ax.add_patch(
+                patches.Circle(
+                    (ox, oy), r,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        plt.plot(self.xI[0], self.xI[1], "bs", linewidth=3)
+        plt.plot(self.xG[0], self.xG[1], "gs", linewidth=3)
+        plt.title(name)
+        plt.axis("equal")
+
+    @staticmethod
+    def plot_visited(nodelist, animation):
+        if animation:
+            for node in nodelist:
+                if node.parent:
+                    plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
+                    plt.gcf().canvas.mpl_connect('key_release_event',
+                                                 lambda event: [exit(0) if event.key == 'escape' else None])
+                    plt.pause(0.001)
+        else:
+            for node in nodelist:
+                if node.parent:
+                    plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
+
+
+    @staticmethod
+    def plot_path(path):
+        plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2)
+        plt.pause(0.01)
+        plt.show()

Sampling-based Planning/rrt_2D/rrt*.py

@@ -0,0 +1,200 @@
+from rrt_2D import env
+from rrt_2D import plotting
+
+import numpy as np
+import math
+
+
+class Node:
+    def __init__(self, n):
+        self.x = n[0]
+        self.y = n[1]
+        self.cost = 0.0
+        self.parent = None
+
+
+class RRT:
+    def __init__(self, xI, xG):
+        self.xI = Node(xI)
+        self.xG = Node(xG)
+        self.expand_len = 1
+        self.goal_sample_rate = 0.05
+        self.connect_dist = 10
+        self.iterations = 5000
+        self.node_list = [self.xI]
+
+        self.env = env.Env()
+        self.plotting = plotting.Plotting(xI, xG)
+
+        self.x_range = self.env.x_range
+        self.y_range = self.env.y_range
+        self.obs_circle = self.env.obs_circle
+        self.obs_rectangle = self.env.obs_rectangle
+        self.obs_boundary = self.env.obs_boundary
+
+        self.path = self.planning()
+        self.plotting.animation(self.node_list, self.path, False)
+
+    def planning(self):
+        for k in range(self.iterations):
+            node_rand = self.random_state()
+            node_near = self.nearest_neighbor(self.node_list, node_rand)
+            node_new = self.new_state(node_near, node_rand)
+
+            if not self.check_collision(node_new):
+                neighbor_index = self.find_near_neighbor(node_new)
+                node_new = self.choose_parent(node_new, neighbor_index)
+                if node_new:
+                    self.node_list.append(node_new)
+                    self.rewire(node_new, neighbor_index)
+
+            # if self.dis_to_goal(self.node_list[-1]) <= self.expand_len:
+            #     self.new_state(self.node_list[-1], self.xG)
+            #     return self.extract_path()
+
+        index = self.search_best_goal_node()
+        self.xG.parent = self.node_list[index]
+        return self.extract_path()
+
+    def random_state(self):
+        if np.random.random() > self.goal_sample_rate:
+            return Node((np.random.uniform(self.x_range[0], self.x_range[1]),
+                         np.random.uniform(self.y_range[0], self.y_range[1])))
+        return self.xG
+
+    def nearest_neighbor(self, node_list, n):
+        return self.node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
+                                             for nd in node_list]))]
+
+    def new_state(self, node_start, node_goal):
+        node_new = Node((node_start.x, node_start.y))
+        dist, theta = self.get_distance_and_angle(node_new, node_goal)
+        dist = min(self.expand_len, dist)
+
+        node_new.x += dist * math.cos(theta)
+        node_new.y += dist * math.sin(theta)
+        node_new.parent = node_start
+
+        return node_new
+
+    def find_near_neighbor(self, node_new):
+        n = len(self.node_list) + 1
+        r = min(self.connect_dist * math.sqrt((math.log(n) / n)), self.expand_len)
+
+        dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.node_list]
+        node_index = [dist_table.index(d) for d in dist_table if d <= r]
+
+        return node_index
+
+    def choose_parent(self, node_new, neighbor_index):
+        if not neighbor_index:
+            return None
+
+        cost = []
+
+        for i in neighbor_index:
+            node_near = self.node_list[i]
+            node_mid = self.new_state(node_near, node_new)
+
+            if node_mid and not self.check_collision(node_mid):
+                cost.append(self.update_cost(node_near, node_mid))
+            else:
+                cost.append(float("inf"))
+
+        if min(cost) != float('inf'):
+            index = int(np.argmin(cost))
+            neighbor_min = neighbor_index[index]
+            node_new = self.new_state(self.node_list[neighbor_min], node_new)
+            node_new.cost = min(cost)
+            return node_new
+
+        return None
+
+    def search_best_goal_node(self):
+        dist_to_goal_list = [self.dis_to_goal(n) for n in self.node_list]
+        goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.expand_len]
+
+        return goal_inds[0]
+        # safe_goal_inds = []
+        # for goal_ind in goal_inds:
+        #     t_node = self.new_state(self.node_list[goal_ind], self.xG)
+        #     if self.check_collision(t_node):
+        #         safe_goal_inds.append(goal_ind)
+        #
+        # if not safe_goal_inds:
+        #     print('hahhah')
+        #     return None
+        #
+        # min_cost = min([self.node_list[i].cost for i in safe_goal_inds])
+        # for i in safe_goal_inds:
+        #     if self.node_list[i].cost == min_cost:
+        #         self.xG.parent = self.node_list[i]
+
+    def rewire(self, node_new, neighbor_index):
+        for i in neighbor_index:
+            node_near = self.node_list[i]
+            node_edge = self.new_state(node_new, node_near)
+            if not node_edge:
+                continue
+
+            node_edge.cost = self.update_cost(node_new, node_near)
+            collision = self.check_collision(node_edge)
+            improved_cost = node_near.cost > node_edge.cost
+
+            if not collision and improved_cost:
+                self.node_list[i] = node_edge
+                self.propagate_cost_to_leaves(node_new)
+
+    def update_cost(self, node_start, node_end):
+        dist, theta = self.get_distance_and_angle(node_start, node_end)
+        return node_start.cost + dist
+
+    def propagate_cost_to_leaves(self, parent_node):
+        for node in self.node_list:
+            if node.parent == parent_node:
+                node.cost = self.update_cost(parent_node, node)
+                self.propagate_cost_to_leaves(node)
+
+    def extract_path(self):
+        path = [[self.xG.x, self.xG.y]]
+        node = self.xG
+        while node.parent is not None:
+            path.append([node.x, node.y])
+            node = node.parent
+        path.append([node.x, node.y])
+
+        return path
+
+    def dis_to_goal(self, node_cal):
+        return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y)
+
+    def check_collision(self, node_end):
+        if node_end is None:
+            return True
+
+        for (ox, oy, r) in self.obs_circle:
+            if math.hypot(node_end.x - ox, node_end.y - oy) <= r:
+                return True
+
+        for (ox, oy, w, h) in self.obs_rectangle:
+            if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
+                return True
+
+        for (ox, oy, w, h) in self.obs_boundary:
+            if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
+                return True
+
+        return False
+
+    @staticmethod
+    def get_distance_and_angle(node_start, node_end):
+        dx = node_end.x - node_start.x
+        dy = node_end.y - node_start.y
+        return math.hypot(dx, dy), math.atan2(dy, dx)
+
+
+if __name__ == '__main__':
+    x_Start = (2, 2)  # Starting node
+    x_Goal = (49, 28)  # Goal node
+
+    rrt = RRT(x_Start, x_Goal)

Sampling-based Planning/rrt_2D/rrt.py

@@ -0,0 +1,114 @@
+from rrt_2D import env
+from rrt_2D import plotting
+
+import numpy as np
+import math
+
+
+class Node:
+    def __init__(self, n):
+        self.x = n[0]
+        self.y = n[1]
+        self.parent = None
+
+
+class RRT:
+    def __init__(self, xI, xG):
+        self.xI = Node(xI)
+        self.xG = Node(xG)
+        self.expand_len = 0.4
+        self.goal_sample_rate = 0.05
+        self.iterations = 5000
+        self.node_list = [self.xI]
+
+        self.env = env.Env()
+        self.plotting = plotting.Plotting(xI, xG)
+
+        self.x_range = self.env.x_range
+        self.y_range = self.env.y_range
+        self.obs_circle = self.env.obs_circle
+        self.obs_rectangle = self.env.obs_rectangle
+        self.obs_boundary = self.env.obs_boundary
+
+        self.path = self.planning()
+        self.plotting.animation(self.node_list, self.path)
+
+    def planning(self):
+        for i in range(self.iterations):
+            node_rand = self.random_state()
+            node_near = self.nearest_neighbor(self.node_list, node_rand)
+            node_new = self.new_state(node_near, node_rand)
+
+            if not self.check_collision(node_new):
+                self.node_list.append(node_new)
+
+            if self.dis_to_goal(self.node_list[-1]) <= self.expand_len:
+                self.new_state(self.node_list[-1], self.xG)
+                return self.extract_path(self.node_list)
+
+        return None
+
+    def random_state(self):
+        if np.random.random() > self.goal_sample_rate:
+            return Node((np.random.uniform(self.x_range[0], self.x_range[1]),
+                         np.random.uniform(self.y_range[0], self.y_range[1])))
+        return self.xG
+
+    def nearest_neighbor(self, node_list, n):
+        return self.node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
+                                             for nd in node_list]))]
+
+    def new_state(self, node_start, node_end):
+        node_new = Node((node_start.x, node_start.y))
+        dist, theta = self.get_distance_and_angle(node_new, node_end)
+
+        dist = min(self.expand_len, dist)
+        node_new.x += dist * math.cos(theta)
+        node_new.y += dist * math.sin(theta)
+        node_new.parent = node_start
+
+        return node_new
+
+    def extract_path(self, nodelist):
+        path = [(self.xG.x, self.xG.y)]
+        node_now = nodelist[-1]
+
+        while node_now.parent is not None:
+            node_now = node_now.parent
+            path.append((node_now.x, node_now.y))
+
+        return path
+
+    def dis_to_goal(self, node_cal):
+        return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y)
+
+    def check_collision(self, node_end):
+        if node_end is None:
+            return True
+
+        for (ox, oy, r) in self.obs_circle:
+            if math.hypot(node_end.x - ox, node_end.y - oy) <= r:
+                return True
+
+        for (ox, oy, w, h) in self.obs_rectangle:
+            if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
+                return True
+
+        for (ox, oy, w, h) in self.obs_boundary:
+            if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
+                return True
+
+        return False
+
+    @staticmethod
+    def get_distance_and_angle(node_start, node_end):
+        dx = node_end.x - node_start.x
+        dy = node_end.y - node_start.y
+        return math.hypot(dx, dy), math.atan2(dy, dx)
+
+
+if __name__ == '__main__':
+    x_Start = (2, 2)  # Starting node
+    x_Goal = (49, 28)  # Goal node
+
+    rrt = RRT(x_Start, x_Goal)

Sampling-based Planning/rrt_3D/env3D.py

@@ -0,0 +1,44 @@
+# this is the three dimensional configuration space for rrt
+# !/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: yue qi
+"""
+import numpy as np
+
+
+def getblocks(resolution):
+    # 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]]
+    Obstacles = []
+    for i in block:
+        i = np.array(i)
+        Obstacles.append((i[0] / resolution, i[1] / resolution, i[2] / resolution, i[3] / resolution, i[4] / resolution,
+                          i[5] / resolution))
+    return np.array(Obstacles)
+
+
+class env():
+    def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1):
+        self.resolution = resolution
+        self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) / resolution
+        self.blocks = getblocks(resolution)
+        self.start = np.array([0.5, 2.5, 5.5])
+        self.goal = np.array([19.0, 2.5, 5.5])
+
+    def visualize(self):
+        # fig = plt.figure()
+        # TODO: do visualizations
+        return
+
+
+if __name__ == '__main__':
+    newenv = env()
+    X = StateSpace(newenv.boundary, newenv.resolution)
+    print(X)

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -0,0 +1,59 @@
+
+"""
+This is rrt star code for 3D
+@author: yue qi
+"""
+import numpy as np
+from numpy.matlib import repmat
+from rrt_3D.env3D import env
+from collections import defaultdict
+import pyrr as pyrr
+from utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path
+import time
+
+
+class rrtstar():
+    def __init__(self):
+        self.env = env()
+        self.Parent = defaultdict(lambda: defaultdict(dict))
+        self.V = []
+        self.E = []
+        self.i = 0
+        self.maxiter = 10000
+        self.stepsize = 0.5
+        self.Path = []
+
+    def wireup(self,x,y):
+        self.E.append([x,y]) # add edge
+        self.Parent[str(x[0])][str(x[1])][str(x[2])] = y
+
+    def removewire(self,xnear):
+        xparent = self.Parent[str(xnear[0])][str(xnear[1])][str(xnear[2])]
+        a = np.array([xnear,xparent])
+        self.E = [xx for xx in self.E if not (xx==a).all()] # remove and replace old the connection
+
+    def run(self):
+        self.V.append(self.env.start)
+        ind = 0
+        xnew = self.env.start
+        while ind < self.maxiter and getDist(xnew,self.env.goal) > 1:
+            xrand    = sampleFree(self)
+            xnearest = nearest(self,xrand)
+            xnew     = steer(self,xnearest,xrand)
+            if not isCollide(self,xnearest,xnew):
+                self.V.append(xnew) # add point
+                self.wireup(xnew,xnearest)
+                #visualization(self)
+                self.i += 1
+            ind += 1
+            if getDist(xnew,self.env.goal) <= 1:
+                self.wireup(self.env.goal,xnew)
+                self.Path,D = path(self)
+                print('Total distance = '+str(D))
+        visualization(self)
+
+if __name__ == '__main__':
+    p = rrtstar()
+    starttime = time.time()
+    p.run()
+    print('time used = ' + str(time.time()-starttime))

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -0,0 +1,79 @@
+
+"""
+This is rrt star code for 3D
+@author: yue qi
+"""
+import numpy as np
+from numpy.matlib import repmat
+from rrt_3D.env3D import env
+from collections import defaultdict
+import pyrr as pyrr
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path
+import time
+
+
+class rrtstar():
+    def __init__(self):
+        self.env = env()
+        self.Parent = defaultdict(lambda: defaultdict(dict))
+        self.V = []
+        self.E = []
+        self.i = 0
+        self.maxiter = 10000
+        self.stepsize = 0.5
+        self.Path = []
+
+    def wireup(self,x,y):
+        self.E.append([x,y]) # add edge
+        self.Parent[str(x[0])][str(x[1])][str(x[2])] = y
+
+    def removewire(self,xnear):
+        xparent = self.Parent[str(xnear[0])][str(xnear[1])][str(xnear[2])]
+        a = np.array([xnear,xparent])
+        self.E = [xx for xx in self.E if not (xx==a).all()] # remove and replace old the connection
+
+    def run(self):
+        self.V.append(self.env.start)
+        ind = 0
+        xnew = self.env.start
+        while ind < self.maxiter and getDist(xnew,self.env.goal) > 1:
+            xrand    = sampleFree(self)
+            xnearest = nearest(self,xrand)
+            xnew     = steer(self,xnearest,xrand)
+            if not isCollide(self,xnearest,xnew):
+                Xnear = near(self,xnew)
+                self.V.append(xnew) # add point
+                # visualization(self)
+                # minimal path and minimal cost
+                xmin,cmin = xnearest,cost(self,xnearest) + getDist(xnearest,xnew)
+                # connecting along minimal cost path
+                if self.i == 0:
+                    c1 = cost(self,Xnear) + getDist(xnew,Xnear)
+                    if not isCollide(self,xnew,Xnear) and c1 < cmin:
+                            xmin,cmin = Xnear,c1
+                    self.wireup(xnew,xmin)
+                else:
+                    for xnear in Xnear:
+                        c1 = cost(self,xnear) + getDist(xnew,xnear)
+                        if not isCollide(self,xnew,xnear) and c1 < cmin:
+                            xmin,cmin = xnear,c1
+                    self.wireup(xnew,xmin)
+                    # rewire
+                    for xnear in Xnear:
+                        c2 = cost(self,xnew) + getDist(xnew,xnear)
+                        if not isCollide(self,xnew,xnear) and c2 < cost(self,xnear):
+                            self.removewire(xnear)
+                            self.wireup(xnear,xnew)
+                self.i += 1
+            ind += 1
+            if getDist(xnew,self.env.goal) <= 1:
+                self.wireup(self.env.goal,xnew)
+                self.Path,D = path(self)
+                print('Total distance = '+str(D))
+        visualization(self)
+
+if __name__ == '__main__':
+    p = rrtstar()
+    starttime = time.time()
+    p.run()
+    print('time used = ' + str(time.time()-starttime))

Sampling-based Planning/rrt_3D/utils3D.py

@@ -0,0 +1,177 @@
+import numpy as np
+from numpy.matlib import repmat
+import pyrr as pyrr
+# plotting
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from mpl_toolkits.mplot3d.art3d import Poly3DCollection
+import mpl_toolkits.mplot3d as plt3d
+
+
+def getRay(x, y):
+    direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]]
+    return np.array([x, direc])
+
+
+def getAABB(blocks):
+    AABB = []
+    for i in 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 getDist(pos1, pos2):
+    return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
+
+
+def draw_block_list(ax, blocks):
+    '''
+    Subroutine used by draw_map() to display the environment blocks
+    '''
+    v = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]],
+                 dtype='float')
+    f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]])
+    # clr = blocks[:,6:]/255
+    n = blocks.shape[0]
+    d = blocks[:, 3:6] - blocks[:, :3]
+    vl = np.zeros((8 * n, 3))
+    fl = np.zeros((6 * n, 4), dtype='int64')
+    # fcl = np.zeros((6*n,3))
+    for k in range(n):
+        vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3]
+        fl[k * 6:(k + 1) * 6, :] = f + k * 8
+        # fcl[k*6:(k+1)*6,:] = clr[k,:]
+
+    if type(ax) is Poly3DCollection:
+        ax.set_verts(vl[fl])
+    else:
+        pc = Poly3DCollection(vl[fl], alpha=0.15, linewidths=1, edgecolors='k')
+        # pc.set_facecolor(fcl)
+        h = ax.add_collection3d(pc)
+        return h
+
+
+''' The following utils can be used for rrt or rrt*,
+    required param initparams should have
+    env,      environement generated from env3D
+    V,        node set
+    E,        edge set
+    i,        nodes added
+    maxiter,  maximum iteration allowed
+    stepsize, leaf growth restriction
+
+'''
+
+
+def sampleFree(initparams):
+    x = np.random.uniform(initparams.env.boundary[0:3], initparams.env.boundary[3:6])
+    if isinside(initparams, x):
+        return sampleFree(initparams)
+    else:
+        return np.array(x)
+
+
+def isinside(initparams, x):
+    '''see if inside obstacle'''
+    for i in initparams.env.blocks:
+        if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]:
+            return True
+    return False
+
+
+def isCollide(initparams, x, y):
+    '''see if line intersects obstacle'''
+    ray = getRay(x, y)
+    dist = getDist(x, y)
+    for i in getAABB(initparams.env.blocks):
+        shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)
+        if shot is not None:
+            dist_wall = getDist(x, shot)
+            if dist_wall <= dist:  # collide
+                return True
+    return False
+
+
+def nearest(initparams, x):
+    V = np.array(initparams.V)
+    if initparams.i == 0:
+        return initparams.V[0]
+    xr = repmat(x, len(V), 1)
+    dists = np.linalg.norm(xr - V, axis=1)
+    return initparams.V[np.argmin(dists)]
+
+
+def steer(initparams, x, y):
+    direc = (y - x) / np.linalg.norm(y - x)
+    xnew = x + initparams.stepsize * direc
+    return xnew
+
+
+def near(initparams, x, r=2):
+    # TODO: r = min{gamma*log(card(V)/card(V)1/d),eta}
+    V = np.array(initparams.V)
+    if initparams.i == 0:
+        return initparams.V[0]
+    xr = repmat(x, len(V), 1)
+    inside = np.linalg.norm(xr - V, axis=1) < r
+    nearpoints = V[inside]
+    return np.array(nearpoints)
+
+
+def cost(initparams, x):
+    '''here use the additive recursive cost function'''
+    if all(x == initparams.env.start):
+        return 0
+    xparent = initparams.Parent[str(x[0])][str(x[1])][str(x[2])]
+    return cost(initparams, xparent) + getDist(x, xparent)
+
+
+def visualization(initparams):
+    V = np.array(initparams.V)
+    E = np.array(initparams.E)
+    Path = np.array(initparams.Path)
+    start = initparams.env.start
+    goal = initparams.env.goal
+    ax = plt.subplot(111, projection='3d')
+    ax.view_init(elev=0., azim=90)
+    ax.clear()
+    draw_block_list(ax, initparams.env.blocks)
+    if E != []:
+        for i in E:
+            xs = i[0][0], i[1][0]
+            ys = i[0][1], i[1][1]
+            zs = i[0][2], i[1][2]
+            line = plt3d.art3d.Line3D(xs, ys, zs)
+            ax.add_line(line)
+
+    if Path != []:
+        for i in Path:
+            xs = i[0][0], i[1][0]
+            ys = i[0][1], i[1][1]
+            zs = i[0][2], i[1][2]
+            line = plt3d.art3d.Line3D(xs, ys, zs, color='r')
+            ax.add_line(line)
+
+    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')
+    ax.scatter3D(V[:, 0], V[:, 1], V[:, 2])
+    plt.xlim(initparams.env.boundary[0], initparams.env.boundary[3])
+    plt.ylim(initparams.env.boundary[1], initparams.env.boundary[4])
+    ax.set_zlim(initparams.env.boundary[2], initparams.env.boundary[5])
+    plt.xlabel('x')
+    plt.ylabel('y')
+    if not Path != []:
+        plt.pause(0.001)
+    else:
+        plt.show()
+
+
+def path(initparams, Path=[], dist=0):
+    x = initparams.env.goal
+    while not all(x == initparams.env.start):
+        x2 = initparams.Parent[str(x[0])][str(x[1])][str(x[2])]
+        Path.append(np.array([x, x2]))
+        dist += getDist(x, x2)
+        x = x2
+    return Path, dist
+