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

rotate y

Browse Source
master
Zhengyu Peng 3 years ago
parent e6b0bee20d
commit 063f73588e
2 changed files with 270 additions and 58 deletions
Show Stats Download Patch File Download Diff File

8
software/rpi/hexapod.py
Unescape Escape View File

@ -16,7 +16,7 @@ from path_generator import gen_fastforward_path, gen_fastbackward_path
from path_generator import gen_leftturn_path, gen_rightturn_path
from path_generator import gen_shiftleft_path, gen_shiftright_path
from path_generator import gen_climb_path
from path_generator import gen_rotatex_path
from path_generator import gen_rotatex_path, gen_rotatey_path
SIN30 = 0.5
@ -103,6 +103,7 @@ class Hexapod:
self.climb_path = gen_climb_path()
self.rotatex_path = gen_rotatex_path(self.standby_coordinate)
self.rotatey_path = gen_rotatey_path(self.standby_coordinate)
self.standby()
time.sleep(1)
@ -134,8 +135,9 @@ class Hexapod:
# for mm in range(0, 20):
# self.move(self.climb_path, 0.005)
self.move(self.rotatex_path, 0.005)
# self.move(self.rotatex_path, 0.005)
for mm in range(0, 20):
self.move(self.rotatey_path, 0.005)
time.sleep(1)
self.standby()

320
software/rpi/path_generator.py
Unescape Escape View File

@ -1,6 +1,6 @@
from lib import semicircle_generator, semicircle2_generator
from lib import path_rotate_z
from lib import get_rotate_x_matrix
from lib import get_rotate_x_matrix, get_rotate_y_matrix
import numpy as np
from collections import deque
@ -283,46 +283,46 @@ def gen_rotatex_path(standby_coordinate):
idx = i+quarter
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
1]*m[2, 1]+standby_coordinate[5, 2]*m[2, 2]+m[2, 3]
result.append(m)
for i in range(quarter):
@ -331,46 +331,46 @@ def gen_rotatex_path(standby_coordinate):
idx = i+quarter*2
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
1]*m[2, 1]+standby_coordinate[5, 2]*m[2, 2]+m[2, 3]
result.append(m)
for i in range(quarter):
@ -379,47 +379,257 @@ def gen_rotatex_path(standby_coordinate):
idx = i+quarter*3
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
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]
1]*m[2, 1]+standby_coordinate[5, 2]*m[2, 2]+m[2, 3]
result.append(m)
return path
# return result, "matrix", 50, (0, quarter*2)
def gen_rotatey_path(standby_coordinate):
# standby_coordinate = np.ones((6,3))
g_steps = 20
swing_angle = 15
x_radius = 15
assert (g_steps % 4) == 0
quarter = int(g_steps/4)
path = np.zeros((g_steps, 6, 3))
result = []
step_angle = swing_angle / quarter
step_offset = x_radius / quarter
for i in range(quarter):
m = get_rotate_y_matrix(swing_angle - i*step_angle)
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]
# result.append(m)
for i in range(quarter):
m = get_rotate_y_matrix(-i*step_angle)
m[1, 3] = -x_radius + i * step_offset
idx = i+quarter
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]
# result.append(m)
for i in range(quarter):
m = get_rotate_y_matrix(i*step_angle-swing_angle)
m[1, 3] = i * step_offset
idx = i+quarter*2
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]
# result.append(m)
for i in range(quarter):
m = get_rotate_y_matrix(i*step_angle)
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,
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]
# result.append(m)
return path
Write Preview
Loading…
Cancel
Save