update

Sampling-based Planning/rrt_3D/plot_util3D.py

@@ -6,21 +6,24 @@ import mpl_toolkits.mplot3d as plt3d
 from mpl_toolkits.mplot3d import proj3d
 import numpy as np
 
-def CreateSphere(center,r):
-    u = np.linspace(0,2* np.pi,30)
-    v = np.linspace(0,np.pi,30)
-    x = np.outer(np.cos(u),np.sin(v))
-    y = np.outer(np.sin(u),np.sin(v))
-    z = np.outer(np.ones(np.size(u)),np.cos(v))
-    x, y, z = r*x + center[0], r*y + center[1], r*z + center[2]
-    return (x,y,z)
-
-def draw_Spheres(ax,balls):
+
+def CreateSphere(center, r):
+    u = np.linspace(0, 2 * np.pi, 30)
+    v = np.linspace(0, np.pi, 30)
+    x = np.outer(np.cos(u), np.sin(v))
+    y = np.outer(np.sin(u), np.sin(v))
+    z = np.outer(np.ones(np.size(u)), np.cos(v))
+    x, y, z = r * x + center[0], r * y + center[1], r * z + center[2]
+    return (x, y, z)
+
+
+def draw_Spheres(ax, balls):
     for i in balls:
-        (xs,ys,zs) = CreateSphere(i[0:3],i[-1])
-        ax.plot_wireframe(xs, ys, zs, alpha=0.15,color="b")
+        (xs, ys, zs) = CreateSphere(i[0:3], i[-1])
+        ax.plot_wireframe(xs, ys, zs, alpha=0.15, color="b")
 
-def draw_block_list(ax, blocks ,color=None,alpha=0.15):
+
+def draw_block_list(ax, blocks, color=None, alpha=0.15):
     '''
     drawing the blocks on the graph
     '''
@@ -42,18 +45,19 @@ 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]])
+    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)
+    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
+    verts = (obb.O @ ori_body.T).T + obb.P
     return verts
 
 
-def draw_obb(ax, OBB, color=None,alpha=0.15):
+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))
@@ -70,7 +74,7 @@ def draw_obb(ax, OBB, color=None,alpha=0.15):
         return h
 
 
-def draw_line(ax,SET,visibility=1,color=None):
+def draw_line(ax, SET, visibility=1, color=None):
     if SET != []:
         for i in SET:
             xs = i[0][0], i[1][0]
@@ -79,8 +83,9 @@ def draw_line(ax,SET,visibility=1,color=None):
             line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color)
             ax.add_line(line)
 
+
 def visualization(initparams):
-    if initparams.ind % 20 == 0 or initparams.done:
+    if initparams.ind % 100 == 0 or initparams.done:
         V = np.array(list(initparams.V))
         E = initparams.E
         Path = np.array(initparams.Path)
@@ -89,33 +94,34 @@ def visualization(initparams):
         edges = E.get_edge()
         # generate axis objects
         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.+ 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.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')
+            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')
         if len(V) > 0:
-            ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',)
+            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') 
+        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)
+        dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
+        ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
         plt.xlabel('x')
         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
@@ -123,12 +129,14 @@ def make_get_proj(self, rx, ry, rz):
     '''
 
     rm = max(rx, ry, rz)
-    kx = rm / rx; ky = rm / ry; kz = rm / 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
+        relev, razim = np.pi * self.elev / 180, np.pi * self.azim / 180
 
         xmin, xmax = self.get_xlim3d()
         ymin, ymax = self.get_ylim3d()
@@ -140,35 +148,37 @@ def make_get_proj(self, rx, ry, rz):
                                              zmin, zmax)
         ratio = 0.5
         # adjust the aspect ratio                          ##
-        aspectM = proj3d.world_transformation(-kx + 1, kx, ##
-                                              -ky + 1, ky, ##
-                                              -kz + 1, kz) ##
+        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
+        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:
+        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
+        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)) ##
+        M0 = np.dot(viewM, np.dot(aspectM, worldM))  ##
         M = np.dot(perspM, M0)
         return M
+
     return get_proj
 
+
 if __name__ == '__main__':
-    pass
+    pass

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -14,7 +14,8 @@ 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, near, visualization, cost, path, edgeset, hash3D, dehash
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset, \
+    hash3D, dehash
 
 
 class rrtstar():
@@ -25,18 +26,18 @@ class rrtstar():
         self.E = edgeset()
         self.i = 0
         self.maxiter = 10000
-        self.stepsize = 0.5
+        self.stepsize = 1.0
         self.Path = []
         self.done = False
 
     def wireup(self, x, y):
-        self.E.add_edge([x,y]) # add edge
+        self.E.add_edge([x, y])  # add edge
         self.Parent[hash3D(x)] = y
 
     def run(self):
         self.V.append(self.env.start)
         self.ind = 0
-        self.fig = plt.figure(figsize = (10,8))
+        self.fig = plt.figure(figsize=(10, 8))
         xnew = self.env.start
         while self.ind < self.maxiter and getDist(xnew, self.env.goal) > 1:
             xrand = sampleFree(self)
@@ -45,7 +46,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:

Sampling-based Planning/rrt_3D/utils3D.py

@@ -4,13 +4,16 @@ import pyrr as pyrr
 
 import os
 import sys
-sys.path.append(os.path.dirname(os.path.abspath(__file__))+"/../../Sampling-based Planning/")
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
 from rrt_3D.plot_util3D import visualization
 
+
 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:
@@ -21,6 +24,7 @@ def getAABB(blocks):
 def getDist(pos1, pos2):
     return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
 
+
 ''' The following utils can be used for rrt or rrt*,
     required param initparams should have
     env,      environement generated from env3D
@@ -40,9 +44,10 @@ def sampleFree(initparams):
     if isinside(initparams, x):
         return sampleFree(initparams)
     else:
-        if i < 0.05:
-           return initparams.env.goal+0.01
-        else: return np.array(x)
+        if i < 0.1:
+            return initparams.env.goal + 1
+        else:
+            return np.array(x)
         return np.array(x)
 
 
@@ -53,16 +58,18 @@ def isinside(initparams, x):
             return True
     return False
 
+
 def isinbound(i, x):
     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)
-    if not isinbound(initparams.env.boundary,y):
+    if not isinbound(initparams.env.boundary, y):
         return True
     for i in getAABB(initparams.env.blocks):
         shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)
@@ -98,7 +105,7 @@ def near(initparams, x):
     cardV = len(initparams.V)
     eta = initparams.eta
     gamma = initparams.gamma
-    r = min(gamma*(np.log(cardV)/cardV),eta)
+    r = min(gamma * (np.log(cardV) / cardV), eta)
     if initparams.done: r = 1
     V = np.array(initparams.V)
     if initparams.i == 0:
@@ -126,31 +133,34 @@ def path(initparams, Path=[], dist=0):
         x = x2
     return Path, dist
 
+
 def hash3D(x):
-    return str(x[0])+' '+str(x[1])+' '+str(x[2])
+    return str(x[0]) + ' ' + str(x[1]) + ' ' + str(x[2])
+
 
 def dehash(x):
     return np.array([float(i) for i in x.split(' ')])
 
+
 class edgeset(object):
     def __init__(self):
         self.E = {}
 
-    def add_edge(self,edge):
-        x, y = hash3D(edge[0]),hash3D(edge[1])
+    def add_edge(self, edge):
+        x, y = hash3D(edge[0]), hash3D(edge[1])
         if x in self.E:
             self.E[x].append(y)
         else:
             self.E[x] = [y]
 
-    def remove_edge(self,edge):
-        x, y = edge[0],edge[1]
+    def remove_edge(self, edge):
+        x, y = edge[0], edge[1]
         self.E[hash3D(x)].remove(hash3D(y))
 
     def get_edge(self):
         edges = []
         for v in self.E:
             for n in self.E[v]:
-                #if (n,v) not in edges:
-                edges.append((dehash(v),dehash(n)))
-        return edges
+                # if (n,v) not in edges:
+                edges.append((dehash(v), dehash(n)))
+        return edges

Search-based Planning/Search_3D/env3D.py

@@ -5,9 +5,11 @@
 @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):
     # x angle: row; y angle: pitch; z angle: yaw
     # generate rotation matrix in SO3
     # RzRyRx = R, ZYX intrinsic rotation
@@ -15,17 +17,21 @@ 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.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 = []
@@ -34,6 +40,7 @@ def getblocks():
         Obstacles.append([j for j in i])
     return np.array(Obstacles)
 
+
 def getAABB(blocks):
     # used for Pyrr package for detecting collision
     AABB = []
@@ -41,15 +48,17 @@ 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
 
+
 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
@@ -59,7 +68,8 @@ 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]])
+        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
@@ -68,96 +78,101 @@ def getAABB2(blocks):
         AABB.append(aabb(i))
     return AABB
 
+
 def getballs():
-    spheres = [[16,2.5,4,2],[10,2.5,1,1]]
+    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]):
+
+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]) 
+        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 = 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.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], 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]])
+                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]]
+                [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
+            # 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]])
+                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
         # (x',t') = (Rx, t)
-        if mode == 'rotation': # this makes an OBB rotate
+        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]])
+            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__':