rrt

Sampling-based Planning/rrt_3D/env3D.py

@@ -5,9 +5,21 @@
 @author: yue qi
 """
 import numpy as np
+# from utils3D import OBB2AABB
 
+def R_matrix(z_angle,y_angle,x_angle):
+    # x angle: row; y angle: pitch; z angle: yaw
+    # generate rotation matrix in SO3
+    # RzRyRx = R, ZYX intrinsic rotation
+    # also (r1,r2,r3) in R3*3 in {W} frame
+    # 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)]])
 
-def getblocks(resolution):
+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],
@@ -19,31 +31,134 @@ def getblocks(resolution):
     Obstacles = []
     for i in block:
         i = np.array(i)
-        Obstacles.append([j/resolution for j in i])
+        Obstacles.append([j for j in i])
     return np.array(Obstacles)
 
-def getballs(resolution):
-    spheres = [[16,2.5,3,2],[10,2.5,1,1]]
+def getAABB(blocks):
+    # used for Pyrr package for detecting collision
+    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
+
+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]]
+
+class obb(object):
+    # P: center point
+    # E: extents
+    # O: Rotation matrix in SO(3), in {w}
+    def __init__(self, P, E, O):
+        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/resolution for j in i])
+        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):
         self.resolution = resolution
-        self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) / resolution
-        self.blocks = getblocks(resolution)
-        self.balls = getballs(resolution)
+        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 
 
-    def visualize(self):
-        # fig = plt.figure()
-        # TODO: do visualizations
-        return
+    def New_block(self):
+        newblock = add_block()
+        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'):
+        # 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
+        # (x',t') = (x + 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
+        # (x',t') = (x + 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]])
+
+            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.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 a,ori
+        # (x',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]
+          
 
 
 if __name__ == '__main__':
     newenv = env()
-    print(newenv.balls)

Sampling-based Planning/rrt_3D/plot_util3D.py

@@ -42,6 +42,34 @@ def draw_block_list(ax, blocks ,color=None,alpha=0.15):
         h = ax.add_collection3d(pc)
         return h
 
+def obb_verts(obb):
+    # 0.017004013061523438 for 1000 iters
+    ori_body = np.array([[1,1,1],[-1,1,1],[-1,-1,1],[1,-1,1],\
+                [1,1,-1],[-1,1,-1],[-1,-1,-1],[1,-1,-1]])
+    # P + (ori * E)
+    ori_body = np.multiply(ori_body,obb.E)
+    # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3)
+    verts = (obb.O@ori_body.T).T + obb.P
+    return verts
+
+
+def draw_obb(ax, OBB, color=None,alpha=0.15):
+    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]])
+    n = OBB.shape[0]
+    vl = np.zeros((8 * n, 3))
+    fl = np.zeros((6 * n, 4), dtype='int64')
+    for k in range(n):
+        vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k])
+        fl[k * 6:(k + 1) * 6, :] = f + k * 8
+    if type(ax) is Poly3DCollection:
+        ax.set_verts(vl[fl])
+    else:
+        pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k')
+        pc.set_facecolor(color)
+        h = ax.add_collection3d(pc)
+        return h
+
+
 def draw_line(ax,SET,visibility=1,color=None):
     if SET != []:
         for i in SET:
@@ -52,8 +80,8 @@ def draw_line(ax,SET,visibility=1,color=None):
             ax.add_line(line)
 
 def visualization(initparams):
-    if initparams.ind % 10 == 0 or initparams.done:
-        V = np.array(initparams.V)
+    if initparams.ind % 20 == 0 or initparams.done:
+        V = np.array(list(initparams.V))
         E = initparams.E
         Path = np.array(initparams.Path)
         start = initparams.env.start
@@ -61,15 +89,21 @@ def visualization(initparams):
         edges = E.get_edge()
         # generate axis objects
         ax = plt.subplot(111, projection='3d')
-        ax.view_init(elev=0., azim=90)
+        #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=-8., azim=180)
         ax.clear()
         # drawing objects
         draw_Spheres(ax, initparams.env.balls)
         draw_block_list(ax, initparams.env.blocks)
+        if initparams.env.OBB is not None:
+            draw_obb(ax,initparams.env.OBB)
         draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0)
         draw_line(ax,edges,visibility=0.25)
         draw_line(ax,Path,color='r')
-        ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',)
+        if len(V) > 0:
+            ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',)
         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
@@ -80,7 +114,7 @@ def visualization(initparams):
         ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy)
         plt.xlabel('x')
         plt.ylabel('y')
-        plt.pause(0.001)
+        plt.pause(0.0001)
 
 def make_get_proj(self, rx, ry, rz):
     '''
@@ -134,4 +168,7 @@ def make_get_proj(self, rx, ry, rz):
         M0 = np.dot(viewM, np.dot(aspectM, worldM)) ##
         M = np.dot(perspM, M0)
         return M
-    return get_proj
+    return get_proj
+
+if __name__ == '__main__':
+    pass

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -45,7 +45,7 @@ class rrtstar():
             if not isCollide(self, xnearest, xnew):
                 self.V.append(xnew)  # add point
                 self.wireup(xnew, xnearest)
-                visualization(self)
+                # visualization(self)
                 self.i += 1
             self.ind += 1
             if getDist(xnew, self.env.goal) <= 1: