'env'

Sampling_based_Planning/rrt_3D/BIT_star3D.py

@@ -26,7 +26,7 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/")
 from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, isinside, isinbound
-from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
+from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 from rrt_3D.queue import MinheapPQ
 
 #---------methods to draw ellipse during sampling
@@ -313,7 +313,7 @@ class BIT_star:
             
             # ax.view_init(elev=0.+ 0.03*self.ind/(2*np.pi), azim=90 + 0.03*self.ind/(2*np.pi))
             # ax.view_init(elev=0., azim=90.)
-            ax.view_init(elev=8., azim=90.)
+            ax.view_init(elev=90., azim=0.)
             # ax.view_init(elev=-8., azim=180)
             ax.clear()
             # drawing objects
@@ -333,11 +333,7 @@ class BIT_star:
             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 = self.env.boundary[0], self.env.boundary[3]
-            ymin, ymax = self.env.boundary[1], self.env.boundary[4]
-            zmin, zmax = self.env.boundary[2], self.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)
+            set_axes_equal(ax)
             make_transparent(ax)
             #plt.xlabel('s')
             #plt.ylabel('y')

Sampling_based_Planning/rrt_3D/FMT_star3D.py

@@ -17,7 +17,7 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/")
 from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide
-from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
+from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 from rrt_3D.queue import MinheapPQ
 
 class FMT_star:
@@ -160,7 +160,7 @@ class FMT_star:
             
             # 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=90.)
+            ax.view_init(elev=90., azim=0.)
             # ax.view_init(elev=-8., azim=180)
             ax.clear()
             # drawing objects
@@ -176,11 +176,7 @@ class FMT_star:
             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 = self.env.boundary[0], self.env.boundary[3]
-            ymin, ymax = self.env.boundary[1], self.env.boundary[4]
-            zmin, zmax = self.env.boundary[2], self.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)
+            set_axes_equal(ax)
             make_transparent(ax)
             #plt.xlabel('x')
             #plt.ylabel('y')

Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py

@@ -12,7 +12,7 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/")
 from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide
-from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
+from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 
 
 class dynamic_rrt_3D:
@@ -211,7 +211,7 @@ class dynamic_rrt_3D:
             ax = plt.subplot(111, projection='3d')
             # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
             # ax.view_init(elev=0., azim=90.)
-            ax.view_init(elev=0., azim=90.)
+            ax.view_init(elev=90., azim=0.)
             ax.clear()
             # drawing objects
             draw_Spheres(ax, self.env.balls)
@@ -226,11 +226,7 @@ class dynamic_rrt_3D:
             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 = self.env.boundary[0], self.env.boundary[3]
-            ymin, ymax = self.env.boundary[1], self.env.boundary[4]
-            zmin, zmax = self.env.boundary[2], self.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)
+            set_axes_equal(ax)
             make_transparent(ax)
             # plt.xlabel('s')
             # plt.ylabel('y')

Sampling_based_Planning/rrt_3D/env3D.py

@@ -21,19 +21,24 @@ def R_matrix(z_angle,y_angle,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.00e+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
     AABB = []
@@ -41,6 +46,16 @@ 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
@@ -61,25 +76,8 @@ class obb(object):
         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
-
 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]) 
@@ -87,13 +85,10 @@ class env():
         self.AABB = getAABB2(self.blocks)
         self.AABB_pyrr = getAABB(self.blocks)
         self.balls = getballs()
-        self.OBB = np.array([obb([2.6,2.5,1],[0.2,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.goal = np.array([5.0, 2.5, 5.5])
+        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):
@@ -105,31 +100,10 @@ class env():
     def move_start(self, x):
         self.start = x
 
-    def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], 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])
@@ -154,13 +128,16 @@ class env():
                             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]]
+        # 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()

Sampling_based_Planning/rrt_3D/informed_rrt_star3D.py

@@ -18,7 +18,7 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/")
 from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, isinside, near, nearest, path
-from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
+from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 from rrt_3D.queue import MinheapPQ
 
 
@@ -203,7 +203,7 @@ class IRRT:
             
             # ax.view_init(elev=0.+ 0.03*self.ind/(2*np.pi), azim=90 + 0.03*self.ind/(2*np.pi))
             # ax.view_init(elev=0., azim=90.)
-            ax.view_init(elev=8., azim=90.)
+            ax.view_init(elev=90., azim=0.)
             # ax.view_init(elev=-8., azim=180)
             ax.clear()
             # drawing objects
@@ -221,11 +221,7 @@ class IRRT:
             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 = self.env.boundary[0], self.env.boundary[3]
-            ymin, ymax = self.env.boundary[1], self.env.boundary[4]
-            zmin, zmax = self.env.boundary[2], self.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)
+            set_axes_equal(ax)
             make_transparent(ax)
             #plt.xlabel('s')
             #plt.ylabel('y')

Sampling_based_Planning/rrt_3D/plot_util3D.py

@@ -106,7 +106,7 @@ def visualization(initparams):
         
         # 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=90.)
+        ax.view_init(elev=90., azim=0.)
         # ax.view_init(elev=-8., azim=180)
         ax.clear()
         # drawing objects
@@ -122,74 +122,40 @@ 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)
+        set_axes_equal(ax)
         make_transparent(ax)
         #plt.xlabel('s')
         #plt.ylabel('y')
         ax.set_axis_off()
         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

Sampling_based_Planning/rrt_3D/rrt3D.py

@@ -44,8 +44,8 @@ class rrt():
         while self.ind < self.maxiter:
             xrand = sampleFree(self)
             xnearest = nearest(self, xrand)
-            xnew = steer(self, xnearest, xrand)
-            collide, _ = isCollide(self, xnearest, xnew)
+            xnew, dist = steer(self, xnearest, xrand)
+            collide, _ = isCollide(self, xnearest, xnew, dist=dist)
             if not collide:
                 self.V.append(xnew)  # add point
                 self.wireup(xnew, xnearest)

Sampling_based_Planning/rrt_3D/rrt_connect3D.py

@@ -16,7 +16,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_ba
 
 from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
-from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
+from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 
 
 class Tree():
@@ -161,7 +161,7 @@ class rrt_connect():
             for i in tree_b.Parent:
                 b_edges.append([i,tree_b.Parent[i]])
             ax = plt.subplot(111, projection='3d')
-            ax.view_init(elev=8., azim=90.)
+            ax.view_init(elev=90., azim=0.)
             ax.clear()
             draw_Spheres(ax, self.env.balls)
             draw_block_list(ax, self.env.blocks)
@@ -173,11 +173,7 @@ class rrt_connect():
             draw_line(ax, Path, color='r')
             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')
-            xmin, xmax = self.env.boundary[0], self.env.boundary[3]
-            ymin, ymax = self.env.boundary[1], self.env.boundary[4]
-            zmin, zmax = self.env.boundary[2], self.env.boundary[5]
-            dx, dy, _ = xmax - xmin, ymax - ymin, zmax - zmin
-            ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
+            set_axes_equal(ax)
             make_transparent(ax)
             ax.set_axis_off()
             plt.pause(0.0001)