shark/hexapod
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

simplify calculation

Browse Source
This commit is contained in:
Zhengyu Peng 2021-12-10 23:15:09 -05:00
parent 3b0bb5219b
commit 62b047b4f0
2 changed files with 61 additions and 423 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
software/rpi/hexapod.py
View File

@ -111,36 +111,41 @@ class Hexapod:
self.standby() self.standby()
time.sleep(1) time.sleep(1)
for mm in range(0, 20):
self.move(self.forward_path, 0.005)
for mm in range(0, 20):
self.move(self.backward_path, 0.005)
for mm in range(0, 20):
self.move(self.fastforward_path, 0.005)
for mm in range(0, 20):
self.move(self.fastbackward_path, 0.005)
for mm in range(0, 20):
self.move(self.leftturn_path, 0.005)
for mm in range(0, 20):
self.move(self.rightturn_path, 0.005)
for mm in range(0, 20):
self.move(self.shiftleft_path, 0.005)
for mm in range(0, 20):
self.move(self.shiftright_path, 0.005)
for mm in range(0, 20):
self.move(self.climb_path, 0.005)
# self.move(self.rotatex_path, 0.005)
# for mm in range(0, 20): # for mm in range(0, 20):
# self.move(self.rotatez_path, 0.005) # self.move(self.forward_path, 0.005)
# for mm in range(0, 20):
# self.move(self.backward_path, 0.005)
# for mm in range(0, 20):
# self.move(self.fastforward_path, 0.005)
# for mm in range(0, 20):
# self.move(self.fastbackward_path, 0.005)
# for mm in range(0, 20):
# self.move(self.leftturn_path, 0.005)
# for mm in range(0, 20):
# self.move(self.rightturn_path, 0.005)
# for mm in range(0, 20):
# self.move(self.shiftleft_path, 0.005)
# for mm in range(0, 20):
# self.move(self.shiftright_path, 0.005)
# for mm in range(0, 20):
# self.move(self.climb_path, 0.005)
for mm in range(0, 20):
self.move(self.rotatex_path, 0.005)
for mm in range(0, 20):
self.move(self.rotatey_path, 0.005)
for mm in range(0, 20):
self.move(self.rotatez_path, 0.005)
for mm in range(0, 20):
self.move(self.twist_path, 0.005)
time.sleep(1) time.sleep(1)
self.standby() self.standby()

419
software/rpi/path_generator.py
View File

@ -236,13 +236,10 @@ def gen_rotatex_path(standby_coordinate,
return path return path
def gen_rotatey_path(standby_coordinate): def gen_rotatey_path(standby_coordinate,
# standby_coordinate = np.ones((6,3)) g_steps = 20,
g_steps = 20 swing_angle = 15,
x_radius = 15):
swing_angle = 15
x_radius = 15
assert (g_steps % 4) == 0 assert (g_steps % 4) == 0
quarter = int(g_steps/4) quarter = int(g_steps/4)
@ -251,209 +248,45 @@ def gen_rotatey_path(standby_coordinate):
step_angle = swing_angle / quarter step_angle = swing_angle / quarter
step_offset = x_radius / quarter step_offset = x_radius / quarter
scx = np.append(standby_coordinate, np.ones((6, 1)), axis=1)
for i in range(quarter): for i in range(quarter):
m = get_rotate_y_matrix(swing_angle - i*step_angle) m = get_rotate_y_matrix(swing_angle - i*step_angle)
m[1, 3] = -i * step_offset m[1, 3] = -i * step_offset
path[i, 0, 0] = standby_coordinate[0, 0]*m[0, 0]+standby_coordinate[0,
1]*m[0, 1]+standby_coordinate[0, 2]*m[0, 2]+m[0, 3]
path[i, 0, 1] = standby_coordinate[0, 0]*m[1, 0]+standby_coordinate[0,
1]*m[1, 1]+standby_coordinate[0, 2]*m[1, 2]+m[1, 3]
path[i, 0, 2] = standby_coordinate[0, 0]*m[2, 0]+standby_coordinate[0,
1]*m[2, 1]+standby_coordinate[0, 2]*m[2, 2]+m[2, 3]
path[i, 1, 0] = standby_coordinate[1, 0]*m[0, 0]+standby_coordinate[1,
1]*m[0, 1]+standby_coordinate[1, 2]*m[0, 2]+m[0, 3]
path[i, 1, 1] = standby_coordinate[1, 0]*m[1, 0]+standby_coordinate[1,
1]*m[1, 1]+standby_coordinate[1, 2]*m[1, 2]+m[1, 3]
path[i, 1, 2] = standby_coordinate[1, 0]*m[2, 0]+standby_coordinate[1,
1]*m[2, 1]+standby_coordinate[1, 2]*m[2, 2]+m[2, 3]
path[i, 2, 0] = standby_coordinate[2, 0]*m[0, 0]+standby_coordinate[2,
1]*m[0, 1]+standby_coordinate[2, 2]*m[0, 2]+m[0, 3]
path[i, 2, 1] = standby_coordinate[2, 0]*m[1, 0]+standby_coordinate[2,
1]*m[1, 1]+standby_coordinate[2, 2]*m[1, 2]+m[1, 3]
path[i, 2, 2] = standby_coordinate[2, 0]*m[2, 0]+standby_coordinate[2,
1]*m[2, 1]+standby_coordinate[2, 2]*m[2, 2]+m[2, 3]
path[i, 3, 0] = standby_coordinate[3, 0]*m[0, 0]+standby_coordinate[3,
1]*m[0, 1]+standby_coordinate[3, 2]*m[0, 2]+m[0, 3]
path[i, 3, 1] = standby_coordinate[3, 0]*m[1, 0]+standby_coordinate[3,
1]*m[1, 1]+standby_coordinate[3, 2]*m[1, 2]+m[1, 3]
path[i, 3, 2] = standby_coordinate[3, 0]*m[2, 0]+standby_coordinate[3,
1]*m[2, 1]+standby_coordinate[3, 2]*m[2, 2]+m[2, 3]
path[i, 4, 0] = standby_coordinate[4, 0]*m[0, 0]+standby_coordinate[4,
1]*m[0, 1]+standby_coordinate[4, 2]*m[0, 2]+m[0, 3]
path[i, 4, 1] = standby_coordinate[4, 0]*m[1, 0]+standby_coordinate[4,
1]*m[1, 1]+standby_coordinate[4, 2]*m[1, 2]+m[1, 3]
path[i, 4, 2] = standby_coordinate[4, 0]*m[2, 0]+standby_coordinate[4,
1]*m[2, 1]+standby_coordinate[4, 2]*m[2, 2]+m[2, 3]
path[i, 5, 0] = standby_coordinate[5, 0]*m[0, 0]+standby_coordinate[5,
1]*m[0, 1]+standby_coordinate[5, 2]*m[0, 2]+m[0, 3]
path[i, 5, 1] = standby_coordinate[5, 0]*m[1, 0]+standby_coordinate[5,
1]*m[1, 1]+standby_coordinate[5, 2]*m[1, 2]+m[1, 3]
path[i, 5, 2] = standby_coordinate[5, 0]*m[2, 0]+standby_coordinate[5,
1]*m[2, 1]+standby_coordinate[5, 2]*m[2, 2]+m[2, 3]
path[i,:,:] = ((np.matmul(m, scx.T)).T)[:,:-1]
for i in range(quarter): for i in range(quarter):
m = get_rotate_y_matrix(-i*step_angle) m = get_rotate_y_matrix(-i*step_angle)
m[1, 3] = -x_radius + i * step_offset m[1, 3] = -x_radius + i * step_offset
idx = i+quarter path[i+quarter,:,:] = ((np.matmul(m, scx.T)).T)[:,:-1]
path[idx, 0, 0] = standby_coordinate[0, 0]*m[0, 0]+standby_coordinate[0,
1]*m[0, 1]+standby_coordinate[0, 2]*m[0, 2]+m[0, 3]
path[idx, 0, 1] = standby_coordinate[0, 0]*m[1, 0]+standby_coordinate[0,
1]*m[1, 1]+standby_coordinate[0, 2]*m[1, 2]+m[1, 3]
path[idx, 0, 2] = standby_coordinate[0, 0]*m[2, 0]+standby_coordinate[0,
1]*m[2, 1]+standby_coordinate[0, 2]*m[2, 2]+m[2, 3]
path[idx, 1, 0] = standby_coordinate[1, 0]*m[0, 0]+standby_coordinate[1,
1]*m[0, 1]+standby_coordinate[1, 2]*m[0, 2]+m[0, 3]
path[idx, 1, 1] = standby_coordinate[1, 0]*m[1, 0]+standby_coordinate[1,
1]*m[1, 1]+standby_coordinate[1, 2]*m[1, 2]+m[1, 3]
path[idx, 1, 2] = standby_coordinate[1, 0]*m[2, 0]+standby_coordinate[1,
1]*m[2, 1]+standby_coordinate[1, 2]*m[2, 2]+m[2, 3]
path[idx, 2, 0] = standby_coordinate[2, 0]*m[0, 0]+standby_coordinate[2,
1]*m[0, 1]+standby_coordinate[2, 2]*m[0, 2]+m[0, 3]
path[idx, 2, 1] = standby_coordinate[2, 0]*m[1, 0]+standby_coordinate[2,
1]*m[1, 1]+standby_coordinate[2, 2]*m[1, 2]+m[1, 3]
path[idx, 2, 2] = standby_coordinate[2, 0]*m[2, 0]+standby_coordinate[2,
1]*m[2, 1]+standby_coordinate[2, 2]*m[2, 2]+m[2, 3]
path[idx, 3, 0] = standby_coordinate[3, 0]*m[0, 0]+standby_coordinate[3,
1]*m[0, 1]+standby_coordinate[3, 2]*m[0, 2]+m[0, 3]
path[idx, 3, 1] = standby_coordinate[3, 0]*m[1, 0]+standby_coordinate[3,
1]*m[1, 1]+standby_coordinate[3, 2]*m[1, 2]+m[1, 3]
path[idx, 3, 2] = standby_coordinate[3, 0]*m[2, 0]+standby_coordinate[3,
1]*m[2, 1]+standby_coordinate[3, 2]*m[2, 2]+m[2, 3]
path[idx, 4, 0] = standby_coordinate[4, 0]*m[0, 0]+standby_coordinate[4,
1]*m[0, 1]+standby_coordinate[4, 2]*m[0, 2]+m[0, 3]
path[idx, 4, 1] = standby_coordinate[4, 0]*m[1, 0]+standby_coordinate[4,
1]*m[1, 1]+standby_coordinate[4, 2]*m[1, 2]+m[1, 3]
path[idx, 4, 2] = standby_coordinate[4, 0]*m[2, 0]+standby_coordinate[4,
1]*m[2, 1]+standby_coordinate[4, 2]*m[2, 2]+m[2, 3]
path[idx, 5, 0] = standby_coordinate[5, 0]*m[0, 0]+standby_coordinate[5,
1]*m[0, 1]+standby_coordinate[5, 2]*m[0, 2]+m[0, 3]
path[idx, 5, 1] = standby_coordinate[5, 0]*m[1, 0]+standby_coordinate[5,
1]*m[1, 1]+standby_coordinate[5, 2]*m[1, 2]+m[1, 3]
path[idx, 5, 2] = standby_coordinate[5, 0]*m[2, 0]+standby_coordinate[5,
1]*m[2, 1]+standby_coordinate[5, 2]*m[2, 2]+m[2, 3]
for i in range(quarter): for i in range(quarter):
m = get_rotate_y_matrix(i*step_angle-swing_angle) m = get_rotate_y_matrix(i*step_angle-swing_angle)
m[1, 3] = i * step_offset m[1, 3] = i * step_offset
idx = i+quarter*2
path[idx, 0, 0] = standby_coordinate[0, 0]*m[0, 0]+standby_coordinate[0, path[i+quarter*2,:,:] = ((np.matmul(m, scx.T)).T)[:,:-1]
1]*m[0, 1]+standby_coordinate[0, 2]*m[0, 2]+m[0, 3]
path[idx, 0, 1] = standby_coordinate[0, 0]*m[1, 0]+standby_coordinate[0,
1]*m[1, 1]+standby_coordinate[0, 2]*m[1, 2]+m[1, 3]
path[idx, 0, 2] = standby_coordinate[0, 0]*m[2, 0]+standby_coordinate[0,
1]*m[2, 1]+standby_coordinate[0, 2]*m[2, 2]+m[2, 3]
path[idx, 1, 0] = standby_coordinate[1, 0]*m[0, 0]+standby_coordinate[1,
1]*m[0, 1]+standby_coordinate[1, 2]*m[0, 2]+m[0, 3]
path[idx, 1, 1] = standby_coordinate[1, 0]*m[1, 0]+standby_coordinate[1,
1]*m[1, 1]+standby_coordinate[1, 2]*m[1, 2]+m[1, 3]
path[idx, 1, 2] = standby_coordinate[1, 0]*m[2, 0]+standby_coordinate[1,
1]*m[2, 1]+standby_coordinate[1, 2]*m[2, 2]+m[2, 3]
path[idx, 2, 0] = standby_coordinate[2, 0]*m[0, 0]+standby_coordinate[2,
1]*m[0, 1]+standby_coordinate[2, 2]*m[0, 2]+m[0, 3]
path[idx, 2, 1] = standby_coordinate[2, 0]*m[1, 0]+standby_coordinate[2,
1]*m[1, 1]+standby_coordinate[2, 2]*m[1, 2]+m[1, 3]
path[idx, 2, 2] = standby_coordinate[2, 0]*m[2, 0]+standby_coordinate[2,
1]*m[2, 1]+standby_coordinate[2, 2]*m[2, 2]+m[2, 3]
path[idx, 3, 0] = standby_coordinate[3, 0]*m[0, 0]+standby_coordinate[3,
1]*m[0, 1]+standby_coordinate[3, 2]*m[0, 2]+m[0, 3]
path[idx, 3, 1] = standby_coordinate[3, 0]*m[1, 0]+standby_coordinate[3,
1]*m[1, 1]+standby_coordinate[3, 2]*m[1, 2]+m[1, 3]
path[idx, 3, 2] = standby_coordinate[3, 0]*m[2, 0]+standby_coordinate[3,
1]*m[2, 1]+standby_coordinate[3, 2]*m[2, 2]+m[2, 3]
path[idx, 4, 0] = standby_coordinate[4, 0]*m[0, 0]+standby_coordinate[4,
1]*m[0, 1]+standby_coordinate[4, 2]*m[0, 2]+m[0, 3]
path[idx, 4, 1] = standby_coordinate[4, 0]*m[1, 0]+standby_coordinate[4,
1]*m[1, 1]+standby_coordinate[4, 2]*m[1, 2]+m[1, 3]
path[idx, 4, 2] = standby_coordinate[4, 0]*m[2, 0]+standby_coordinate[4,
1]*m[2, 1]+standby_coordinate[4, 2]*m[2, 2]+m[2, 3]
path[idx, 5, 0] = standby_coordinate[5, 0]*m[0, 0]+standby_coordinate[5,
1]*m[0, 1]+standby_coordinate[5, 2]*m[0, 2]+m[0, 3]
path[idx, 5, 1] = standby_coordinate[5, 0]*m[1, 0]+standby_coordinate[5,
1]*m[1, 1]+standby_coordinate[5, 2]*m[1, 2]+m[1, 3]
path[idx, 5, 2] = standby_coordinate[5, 0]*m[2, 0]+standby_coordinate[5,
1]*m[2, 1]+standby_coordinate[5, 2]*m[2, 2]+m[2, 3]
for i in range(quarter): for i in range(quarter):
m = get_rotate_y_matrix(i*step_angle) m = get_rotate_y_matrix(i*step_angle)
m[1, 3] = x_radius-i * step_offset m[1, 3] = x_radius-i * step_offset
idx = i+quarter*3
path[idx, 0, 0] = standby_coordinate[0, 0]*m[0, 0]+standby_coordinate[0, path[i+quarter*3,:,:] = ((np.matmul(m, scx.T)).T)[:,:-1]
1]*m[0, 1]+standby_coordinate[0, 2]*m[0, 2]+m[0, 3]
path[idx, 0, 1] = standby_coordinate[0, 0]*m[1, 0]+standby_coordinate[0,
1]*m[1, 1]+standby_coordinate[0, 2]*m[1, 2]+m[1, 3]
path[idx, 0, 2] = standby_coordinate[0, 0]*m[2, 0]+standby_coordinate[0,
1]*m[2, 1]+standby_coordinate[0, 2]*m[2, 2]+m[2, 3]
path[idx, 1, 0] = standby_coordinate[1, 0]*m[0, 0]+standby_coordinate[1,
1]*m[0, 1]+standby_coordinate[1, 2]*m[0, 2]+m[0, 3]
path[idx, 1, 1] = standby_coordinate[1, 0]*m[1, 0]+standby_coordinate[1,
1]*m[1, 1]+standby_coordinate[1, 2]*m[1, 2]+m[1, 3]
path[idx, 1, 2] = standby_coordinate[1, 0]*m[2, 0]+standby_coordinate[1,
1]*m[2, 1]+standby_coordinate[1, 2]*m[2, 2]+m[2, 3]
path[idx, 2, 0] = standby_coordinate[2, 0]*m[0, 0]+standby_coordinate[2,
1]*m[0, 1]+standby_coordinate[2, 2]*m[0, 2]+m[0, 3]
path[idx, 2, 1] = standby_coordinate[2, 0]*m[1, 0]+standby_coordinate[2,
1]*m[1, 1]+standby_coordinate[2, 2]*m[1, 2]+m[1, 3]
path[idx, 2, 2] = standby_coordinate[2, 0]*m[2, 0]+standby_coordinate[2,
1]*m[2, 1]+standby_coordinate[2, 2]*m[2, 2]+m[2, 3]
path[idx, 3, 0] = standby_coordinate[3, 0]*m[0, 0]+standby_coordinate[3,
1]*m[0, 1]+standby_coordinate[3, 2]*m[0, 2]+m[0, 3]
path[idx, 3, 1] = standby_coordinate[3, 0]*m[1, 0]+standby_coordinate[3,
1]*m[1, 1]+standby_coordinate[3, 2]*m[1, 2]+m[1, 3]
path[idx, 3, 2] = standby_coordinate[3, 0]*m[2, 0]+standby_coordinate[3,
1]*m[2, 1]+standby_coordinate[3, 2]*m[2, 2]+m[2, 3]
path[idx, 4, 0] = standby_coordinate[4, 0]*m[0, 0]+standby_coordinate[4,
1]*m[0, 1]+standby_coordinate[4, 2]*m[0, 2]+m[0, 3]
path[idx, 4, 1] = standby_coordinate[4, 0]*m[1, 0]+standby_coordinate[4,
1]*m[1, 1]+standby_coordinate[4, 2]*m[1, 2]+m[1, 3]
path[idx, 4, 2] = standby_coordinate[4, 0]*m[2, 0]+standby_coordinate[4,
1]*m[2, 1]+standby_coordinate[4, 2]*m[2, 2]+m[2, 3]
path[idx, 5, 0] = standby_coordinate[5, 0]*m[0, 0]+standby_coordinate[5,
1]*m[0, 1]+standby_coordinate[5, 2]*m[0, 2]+m[0, 3]
path[idx, 5, 1] = standby_coordinate[5, 0]*m[1, 0]+standby_coordinate[5,
1]*m[1, 1]+standby_coordinate[5, 2]*m[1, 2]+m[1, 3]
path[idx, 5, 2] = standby_coordinate[5, 0]*m[2, 0]+standby_coordinate[5,
1]*m[2, 1]+standby_coordinate[5, 2]*m[2, 2]+m[2, 3]
return path return path
def gen_rotatez_path(standby_coordinate): def gen_rotatez_path(standby_coordinate,
# standby_coordinate = np.ones((6,3)) g_steps = 20,
g_steps = 20 z_lift = 4.5,
xy_radius = 1):
z_lift = 4.5
xy_radius = 1
assert (g_steps % 4) == 0 assert (g_steps % 4) == 0
path = np.zeros((g_steps, 6, 3)) path = np.zeros((g_steps, 6, 3))
step_angle = 2*np.pi / g_steps step_angle = 2*np.pi / g_steps
scx = np.append(standby_coordinate, np.ones((6, 1)), axis=1)
for i in range(g_steps): for i in range(g_steps):
x = xy_radius * np.cos(i*step_angle) x = xy_radius * np.cos(i*step_angle)
@ -461,61 +294,23 @@ def gen_rotatez_path(standby_coordinate):
m = get_rotate_y_matrix(np.arctan2( m = get_rotate_y_matrix(np.arctan2(
x, z_lift)*180/np.pi) * get_rotate_x_matrix(np.arctan2(y, z_lift)*180/np.pi) x, z_lift)*180/np.pi) * get_rotate_x_matrix(np.arctan2(y, z_lift)*180/np.pi)
path[i, 0, 0] = standby_coordinate[0, 0]*m[0, 0]+standby_coordinate[0,
1]*m[0, 1]+standby_coordinate[0, 2]*m[0, 2]+m[0, 3]
path[i, 0, 1] = standby_coordinate[0, 0]*m[1, 0]+standby_coordinate[0,
1]*m[1, 1]+standby_coordinate[0, 2]*m[1, 2]+m[1, 3]
path[i, 0, 2] = standby_coordinate[0, 0]*m[2, 0]+standby_coordinate[0,
1]*m[2, 1]+standby_coordinate[0, 2]*m[2, 2]+m[2, 3]
path[i, 1, 0] = standby_coordinate[1, 0]*m[0, 0]+standby_coordinate[1, path[i,:,:] = ((np.matmul(m, scx.T)).T)[:,:-1]
1]*m[0, 1]+standby_coordinate[1, 2]*m[0, 2]+m[0, 3]
path[i, 1, 1] = standby_coordinate[1, 0]*m[1, 0]+standby_coordinate[1,
1]*m[1, 1]+standby_coordinate[1, 2]*m[1, 2]+m[1, 3]
path[i, 1, 2] = standby_coordinate[1, 0]*m[2, 0]+standby_coordinate[1,
1]*m[2, 1]+standby_coordinate[1, 2]*m[2, 2]+m[2, 3]
path[i, 2, 0] = standby_coordinate[2, 0]*m[0, 0]+standby_coordinate[2,
1]*m[0, 1]+standby_coordinate[2, 2]*m[0, 2]+m[0, 3]
path[i, 2, 1] = standby_coordinate[2, 0]*m[1, 0]+standby_coordinate[2,
1]*m[1, 1]+standby_coordinate[2, 2]*m[1, 2]+m[1, 3]
path[i, 2, 2] = standby_coordinate[2, 0]*m[2, 0]+standby_coordinate[2,
1]*m[2, 1]+standby_coordinate[2, 2]*m[2, 2]+m[2, 3]
path[i, 3, 0] = standby_coordinate[3, 0]*m[0, 0]+standby_coordinate[3,
1]*m[0, 1]+standby_coordinate[3, 2]*m[0, 2]+m[0, 3]
path[i, 3, 1] = standby_coordinate[3, 0]*m[1, 0]+standby_coordinate[3,
1]*m[1, 1]+standby_coordinate[3, 2]*m[1, 2]+m[1, 3]
path[i, 3, 2] = standby_coordinate[3, 0]*m[2, 0]+standby_coordinate[3,
1]*m[2, 1]+standby_coordinate[3, 2]*m[2, 2]+m[2, 3]
path[i, 4, 0] = standby_coordinate[4, 0]*m[0, 0]+standby_coordinate[4,
1]*m[0, 1]+standby_coordinate[4, 2]*m[0, 2]+m[0, 3]
path[i, 4, 1] = standby_coordinate[4, 0]*m[1, 0]+standby_coordinate[4,
1]*m[1, 1]+standby_coordinate[4, 2]*m[1, 2]+m[1, 3]
path[i, 4, 2] = standby_coordinate[4, 0]*m[2, 0]+standby_coordinate[4,
1]*m[2, 1]+standby_coordinate[4, 2]*m[2, 2]+m[2, 3]
path[i, 5, 0] = standby_coordinate[5, 0]*m[0, 0]+standby_coordinate[5,
1]*m[0, 1]+standby_coordinate[5, 2]*m[0, 2]+m[0, 3]
path[i, 5, 1] = standby_coordinate[5, 0]*m[1, 0]+standby_coordinate[5,
1]*m[1, 1]+standby_coordinate[5, 2]*m[1, 2]+m[1, 3]
path[i, 5, 2] = standby_coordinate[5, 0]*m[2, 0]+standby_coordinate[5,
1]*m[2, 1]+standby_coordinate[5, 2]*m[2, 2]+m[2, 3]
return path return path
def gen_twist_path(standby_coordinate): def gen_twist_path(standby_coordinate,
g_steps = 20 g_steps = 20,
raise_angle = 3 raise_angle = 3,
twist_x_angle = 20 twist_x_angle = 20,
twise_y_angle = 12 twise_y_angle = 12):
assert (g_steps % 4) == 0 assert (g_steps % 4) == 0
quarter = int(g_steps / 4) quarter = int(g_steps / 4)
step_x_angle = twist_x_angle / quarter step_x_angle = twist_x_angle / quarter
step_y_angle = twise_y_angle / quarter step_y_angle = twise_y_angle / quarter
scx = np.append(standby_coordinate, np.ones((6, 1)), axis=1)
m = get_rotate_x_matrix(raise_angle) m = get_rotate_x_matrix(raise_angle)
@ -525,186 +320,24 @@ def gen_twist_path(standby_coordinate):
temp = m * get_rotate_z_matrix(i*step_x_angle) * \ temp = m * get_rotate_z_matrix(i*step_x_angle) * \
get_rotate_x_matrix(i*step_y_angle) get_rotate_x_matrix(i*step_y_angle)
idx = i+quarter*0 path[i,:,:] = ((np.matmul(temp, scx.T)).T)[:,:-1]
path[idx, 0, 0] = standby_coordinate[0, 0]*temp[0, 0]+standby_coordinate[0,
1]*temp[0, 1]+standby_coordinate[0, 2]*temp[0, 2]+temp[0, 3]
path[idx, 0, 1] = standby_coordinate[0, 0]*temp[1, 0]+standby_coordinate[0,
1]*temp[1, 1]+standby_coordinate[0, 2]*temp[1, 2]+temp[1, 3]
path[idx, 0, 2] = standby_coordinate[0, 0]*temp[2, 0]+standby_coordinate[0,
1]*temp[2, 1]+standby_coordinate[0, 2]*temp[2, 2]+temp[2, 3]
path[idx, 1, 0] = standby_coordinate[1, 0]*temp[0, 0]+standby_coordinate[1,
1]*temp[0, 1]+standby_coordinate[1, 2]*temp[0, 2]+temp[0, 3]
path[idx, 1, 1] = standby_coordinate[1, 0]*temp[1, 0]+standby_coordinate[1,
1]*temp[1, 1]+standby_coordinate[1, 2]*temp[1, 2]+temp[1, 3]
path[idx, 1, 2] = standby_coordinate[1, 0]*temp[2, 0]+standby_coordinate[1,
1]*temp[2, 1]+standby_coordinate[1, 2]*temp[2, 2]+temp[2, 3]
path[idx, 2, 0] = standby_coordinate[2, 0]*temp[0, 0]+standby_coordinate[2,
1]*temp[0, 1]+standby_coordinate[2, 2]*temp[0, 2]+temp[0, 3]
path[idx, 2, 1] = standby_coordinate[2, 0]*temp[1, 0]+standby_coordinate[2,
1]*temp[1, 1]+standby_coordinate[2, 2]*temp[1, 2]+temp[1, 3]
path[idx, 2, 2] = standby_coordinate[2, 0]*temp[2, 0]+standby_coordinate[2,
1]*temp[2, 1]+standby_coordinate[2, 2]*temp[2, 2]+temp[2, 3]
path[idx, 3, 0] = standby_coordinate[3, 0]*temp[0, 0]+standby_coordinate[3,
1]*temp[0, 1]+standby_coordinate[3, 2]*temp[0, 2]+temp[0, 3]
path[idx, 3, 1] = standby_coordinate[3, 0]*temp[1, 0]+standby_coordinate[3,
1]*temp[1, 1]+standby_coordinate[3, 2]*temp[1, 2]+temp[1, 3]
path[idx, 3, 2] = standby_coordinate[3, 0]*temp[2, 0]+standby_coordinate[3,
1]*temp[2, 1]+standby_coordinate[3, 2]*temp[2, 2]+temp[2, 3]
path[idx, 4, 0] = standby_coordinate[4, 0]*temp[0, 0]+standby_coordinate[4,
1]*temp[0, 1]+standby_coordinate[4, 2]*temp[0, 2]+temp[0, 3]
path[idx, 4, 1] = standby_coordinate[4, 0]*temp[1, 0]+standby_coordinate[4,
1]*temp[1, 1]+standby_coordinate[4, 2]*temp[1, 2]+temp[1, 3]
path[idx, 4, 2] = standby_coordinate[4, 0]*temp[2, 0]+standby_coordinate[4,
1]*temp[2, 1]+standby_coordinate[4, 2]*temp[2, 2]+temp[2, 3]
path[idx, 5, 0] = standby_coordinate[5, 0]*temp[0, 0]+standby_coordinate[5,
1]*temp[0, 1]+standby_coordinate[5, 2]*temp[0, 2]+temp[0, 3]
path[idx, 5, 1] = standby_coordinate[5, 0]*temp[1, 0]+standby_coordinate[5,
1]*temp[1, 1]+standby_coordinate[5, 2]*temp[1, 2]+temp[1, 3]
path[idx, 5, 2] = standby_coordinate[5, 0]*temp[2, 0]+standby_coordinate[5,
1]*temp[2, 1]+standby_coordinate[5, 2]*temp[2, 2]+temp[2, 3]
for i in range(quarter): for i in range(quarter):
temp = m * get_rotate_z_matrix((quarter-i)*step_x_angle) * \ temp = m * get_rotate_z_matrix((quarter-i)*step_x_angle) * \
get_rotate_x_matrix((quarter-i)*step_y_angle) get_rotate_x_matrix((quarter-i)*step_y_angle)
idx = i+quarter*1
path[idx, 0, 0] = standby_coordinate[0, 0]*temp[0, 0]+standby_coordinate[0, path[i+quarter*1,:,:] = ((np.matmul(temp, scx.T)).T)[:,:-1]
1]*temp[0, 1]+standby_coordinate[0, 2]*temp[0, 2]+temp[0, 3]
path[idx, 0, 1] = standby_coordinate[0, 0]*temp[1, 0]+standby_coordinate[0,
1]*temp[1, 1]+standby_coordinate[0, 2]*temp[1, 2]+temp[1, 3]
path[idx, 0, 2] = standby_coordinate[0, 0]*temp[2, 0]+standby_coordinate[0,
1]*temp[2, 1]+standby_coordinate[0, 2]*temp[2, 2]+temp[2, 3]
path[idx, 1, 0] = standby_coordinate[1, 0]*temp[0, 0]+standby_coordinate[1,
1]*temp[0, 1]+standby_coordinate[1, 2]*temp[0, 2]+temp[0, 3]
path[idx, 1, 1] = standby_coordinate[1, 0]*temp[1, 0]+standby_coordinate[1,
1]*temp[1, 1]+standby_coordinate[1, 2]*temp[1, 2]+temp[1, 3]
path[idx, 1, 2] = standby_coordinate[1, 0]*temp[2, 0]+standby_coordinate[1,
1]*temp[2, 1]+standby_coordinate[1, 2]*temp[2, 2]+temp[2, 3]
path[idx, 2, 0] = standby_coordinate[2, 0]*temp[0, 0]+standby_coordinate[2,
1]*temp[0, 1]+standby_coordinate[2, 2]*temp[0, 2]+temp[0, 3]
path[idx, 2, 1] = standby_coordinate[2, 0]*temp[1, 0]+standby_coordinate[2,
1]*temp[1, 1]+standby_coordinate[2, 2]*temp[1, 2]+temp[1, 3]
path[idx, 2, 2] = standby_coordinate[2, 0]*temp[2, 0]+standby_coordinate[2,
1]*temp[2, 1]+standby_coordinate[2, 2]*temp[2, 2]+temp[2, 3]
path[idx, 3, 0] = standby_coordinate[3, 0]*temp[0, 0]+standby_coordinate[3,
1]*temp[0, 1]+standby_coordinate[3, 2]*temp[0, 2]+temp[0, 3]
path[idx, 3, 1] = standby_coordinate[3, 0]*temp[1, 0]+standby_coordinate[3,
1]*temp[1, 1]+standby_coordinate[3, 2]*temp[1, 2]+temp[1, 3]
path[idx, 3, 2] = standby_coordinate[3, 0]*temp[2, 0]+standby_coordinate[3,
1]*temp[2, 1]+standby_coordinate[3, 2]*temp[2, 2]+temp[2, 3]
path[idx, 4, 0] = standby_coordinate[4, 0]*temp[0, 0]+standby_coordinate[4,
1]*temp[0, 1]+standby_coordinate[4, 2]*temp[0, 2]+temp[0, 3]
path[idx, 4, 1] = standby_coordinate[4, 0]*temp[1, 0]+standby_coordinate[4,
1]*temp[1, 1]+standby_coordinate[4, 2]*temp[1, 2]+temp[1, 3]
path[idx, 4, 2] = standby_coordinate[4, 0]*temp[2, 0]+standby_coordinate[4,
1]*temp[2, 1]+standby_coordinate[4, 2]*temp[2, 2]+temp[2, 3]
path[idx, 5, 0] = standby_coordinate[5, 0]*temp[0, 0]+standby_coordinate[5,
1]*temp[0, 1]+standby_coordinate[5, 2]*temp[0, 2]+temp[0, 3]
path[idx, 5, 1] = standby_coordinate[5, 0]*temp[1, 0]+standby_coordinate[5,
1]*temp[1, 1]+standby_coordinate[5, 2]*temp[1, 2]+temp[1, 3]
path[idx, 5, 2] = standby_coordinate[5, 0]*temp[2, 0]+standby_coordinate[5,
1]*temp[2, 1]+standby_coordinate[5, 2]*temp[2, 2]+temp[2, 3]
for i in range(quarter): for i in range(quarter):
temp = m * get_rotate_z_matrix(-i*step_x_angle) * \ temp = m * get_rotate_z_matrix(-i*step_x_angle) * \
get_rotate_x_matrix(i*step_y_angle) get_rotate_x_matrix(i*step_y_angle)
idx = i+quarter*2
path[idx, 0, 0] = standby_coordinate[0, 0]*temp[0, 0]+standby_coordinate[0, path[i+quarter*2,:,:] = ((np.matmul(temp, scx.T)).T)[:,:-1]
1]*temp[0, 1]+standby_coordinate[0, 2]*temp[0, 2]+temp[0, 3]
path[idx, 0, 1] = standby_coordinate[0, 0]*temp[1, 0]+standby_coordinate[0,
1]*temp[1, 1]+standby_coordinate[0, 2]*temp[1, 2]+temp[1, 3]
path[idx, 0, 2] = standby_coordinate[0, 0]*temp[2, 0]+standby_coordinate[0,
1]*temp[2, 1]+standby_coordinate[0, 2]*temp[2, 2]+temp[2, 3]
path[idx, 1, 0] = standby_coordinate[1, 0]*temp[0, 0]+standby_coordinate[1,
1]*temp[0, 1]+standby_coordinate[1, 2]*temp[0, 2]+temp[0, 3]
path[idx, 1, 1] = standby_coordinate[1, 0]*temp[1, 0]+standby_coordinate[1,
1]*temp[1, 1]+standby_coordinate[1, 2]*temp[1, 2]+temp[1, 3]
path[idx, 1, 2] = standby_coordinate[1, 0]*temp[2, 0]+standby_coordinate[1,
1]*temp[2, 1]+standby_coordinate[1, 2]*temp[2, 2]+temp[2, 3]
path[idx, 2, 0] = standby_coordinate[2, 0]*temp[0, 0]+standby_coordinate[2,
1]*temp[0, 1]+standby_coordinate[2, 2]*temp[0, 2]+temp[0, 3]
path[idx, 2, 1] = standby_coordinate[2, 0]*temp[1, 0]+standby_coordinate[2,
1]*temp[1, 1]+standby_coordinate[2, 2]*temp[1, 2]+temp[1, 3]
path[idx, 2, 2] = standby_coordinate[2, 0]*temp[2, 0]+standby_coordinate[2,
1]*temp[2, 1]+standby_coordinate[2, 2]*temp[2, 2]+temp[2, 3]
path[idx, 3, 0] = standby_coordinate[3, 0]*temp[0, 0]+standby_coordinate[3,
1]*temp[0, 1]+standby_coordinate[3, 2]*temp[0, 2]+temp[0, 3]
path[idx, 3, 1] = standby_coordinate[3, 0]*temp[1, 0]+standby_coordinate[3,
1]*temp[1, 1]+standby_coordinate[3, 2]*temp[1, 2]+temp[1, 3]
path[idx, 3, 2] = standby_coordinate[3, 0]*temp[2, 0]+standby_coordinate[3,
1]*temp[2, 1]+standby_coordinate[3, 2]*temp[2, 2]+temp[2, 3]
path[idx, 4, 0] = standby_coordinate[4, 0]*temp[0, 0]+standby_coordinate[4,
1]*temp[0, 1]+standby_coordinate[4, 2]*temp[0, 2]+temp[0, 3]
path[idx, 4, 1] = standby_coordinate[4, 0]*temp[1, 0]+standby_coordinate[4,
1]*temp[1, 1]+standby_coordinate[4, 2]*temp[1, 2]+temp[1, 3]
path[idx, 4, 2] = standby_coordinate[4, 0]*temp[2, 0]+standby_coordinate[4,
1]*temp[2, 1]+standby_coordinate[4, 2]*temp[2, 2]+temp[2, 3]
path[idx, 5, 0] = standby_coordinate[5, 0]*temp[0, 0]+standby_coordinate[5,
1]*temp[0, 1]+standby_coordinate[5, 2]*temp[0, 2]+temp[0, 3]
path[idx, 5, 1] = standby_coordinate[5, 0]*temp[1, 0]+standby_coordinate[5,
1]*temp[1, 1]+standby_coordinate[5, 2]*temp[1, 2]+temp[1, 3]
path[idx, 5, 2] = standby_coordinate[5, 0]*temp[2, 0]+standby_coordinate[5,
1]*temp[2, 1]+standby_coordinate[5, 2]*temp[2, 2]+temp[2, 3]
for i in range(quarter): for i in range(quarter):
temp = m * get_rotate_z_matrix((-quarter+i)*step_x_angle) * \ temp = m * get_rotate_z_matrix((-quarter+i)*step_x_angle) * \
get_rotate_x_matrix((quarter-i)*step_y_angle) get_rotate_x_matrix((quarter-i)*step_y_angle)
idx = i+quarter*3
path[idx, 0, 0] = standby_coordinate[0, 0]*temp[0, 0]+standby_coordinate[0, path[i+quarter*3,:,:] = ((np.matmul(temp, scx.T)).T)[:,:-1]
1]*temp[0, 1]+standby_coordinate[0, 2]*temp[0, 2]+temp[0, 3]
path[idx, 0, 1] = standby_coordinate[0, 0]*temp[1, 0]+standby_coordinate[0,
1]*temp[1, 1]+standby_coordinate[0, 2]*temp[1, 2]+temp[1, 3]
path[idx, 0, 2] = standby_coordinate[0, 0]*temp[2, 0]+standby_coordinate[0,
1]*temp[2, 1]+standby_coordinate[0, 2]*temp[2, 2]+temp[2, 3]
path[idx, 1, 0] = standby_coordinate[1, 0]*temp[0, 0]+standby_coordinate[1,
1]*temp[0, 1]+standby_coordinate[1, 2]*temp[0, 2]+temp[0, 3]
path[idx, 1, 1] = standby_coordinate[1, 0]*temp[1, 0]+standby_coordinate[1,
1]*temp[1, 1]+standby_coordinate[1, 2]*temp[1, 2]+temp[1, 3]
path[idx, 1, 2] = standby_coordinate[1, 0]*temp[2, 0]+standby_coordinate[1,
1]*temp[2, 1]+standby_coordinate[1, 2]*temp[2, 2]+temp[2, 3]
path[idx, 2, 0] = standby_coordinate[2, 0]*temp[0, 0]+standby_coordinate[2,
1]*temp[0, 1]+standby_coordinate[2, 2]*temp[0, 2]+temp[0, 3]
path[idx, 2, 1] = standby_coordinate[2, 0]*temp[1, 0]+standby_coordinate[2,
1]*temp[1, 1]+standby_coordinate[2, 2]*temp[1, 2]+temp[1, 3]
path[idx, 2, 2] = standby_coordinate[2, 0]*temp[2, 0]+standby_coordinate[2,
1]*temp[2, 1]+standby_coordinate[2, 2]*temp[2, 2]+temp[2, 3]
path[idx, 3, 0] = standby_coordinate[3, 0]*temp[0, 0]+standby_coordinate[3,
1]*temp[0, 1]+standby_coordinate[3, 2]*temp[0, 2]+temp[0, 3]
path[idx, 3, 1] = standby_coordinate[3, 0]*temp[1, 0]+standby_coordinate[3,
1]*temp[1, 1]+standby_coordinate[3, 2]*temp[1, 2]+temp[1, 3]
path[idx, 3, 2] = standby_coordinate[3, 0]*temp[2, 0]+standby_coordinate[3,
1]*temp[2, 1]+standby_coordinate[3, 2]*temp[2, 2]+temp[2, 3]
path[idx, 4, 0] = standby_coordinate[4, 0]*temp[0, 0]+standby_coordinate[4,
1]*temp[0, 1]+standby_coordinate[4, 2]*temp[0, 2]+temp[0, 3]
path[idx, 4, 1] = standby_coordinate[4, 0]*temp[1, 0]+standby_coordinate[4,
1]*temp[1, 1]+standby_coordinate[4, 2]*temp[1, 2]+temp[1, 3]
path[idx, 4, 2] = standby_coordinate[4, 0]*temp[2, 0]+standby_coordinate[4,
1]*temp[2, 1]+standby_coordinate[4, 2]*temp[2, 2]+temp[2, 3]
path[idx, 5, 0] = standby_coordinate[5, 0]*temp[0, 0]+standby_coordinate[5,
1]*temp[0, 1]+standby_coordinate[5, 2]*temp[0, 2]+temp[0, 3]
path[idx, 5, 1] = standby_coordinate[5, 0]*temp[1, 0]+standby_coordinate[5,
1]*temp[1, 1]+standby_coordinate[5, 2]*temp[1, 2]+temp[1, 3]
path[idx, 5, 2] = standby_coordinate[5, 0]*temp[2, 0]+standby_coordinate[5,
1]*temp[2, 1]+standby_coordinate[5, 2]*temp[2, 2]+temp[2, 3]
return path return path