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

twist

Browse Source
master
Zhengyu Peng 3 years ago
parent f754e3e812
commit 0edcd61109
2 changed files with 212 additions and 4 deletions
Show Stats Download Patch File Download Diff File

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

@ -17,6 +17,7 @@ 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, gen_rotatey_path, gen_rotatez_path
from path_generator import gen_twist_path
SIN30 = 0.5
@ -105,6 +106,7 @@ class Hexapod:
self.rotatex_path = gen_rotatex_path(self.standby_coordinate)
self.rotatey_path = gen_rotatey_path(self.standby_coordinate)
self.rotatez_path = gen_rotatez_path(self.standby_coordinate)
self.twist_path = gen_twist_path(self.standby_coordinate)
self.standby()
time.sleep(1)
@ -138,7 +140,7 @@ class Hexapod:
# self.move(self.rotatex_path, 0.005)
for mm in range(0, 20):
self.move(self.rotatez_path, 0.005)
self.move(self.twist_path, 0.005)
time.sleep(1)
self.standby()

212
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, get_rotate_y_matrix
from lib import get_rotate_x_matrix, get_rotate_y_matrix, get_rotate_z_matrix
import numpy as np
from collections import deque
@ -424,6 +424,7 @@ def gen_rotatex_path(standby_coordinate):
return path
# return result, "matrix", 50, (0, quarter*2)
def gen_rotatey_path(standby_coordinate):
# standby_coordinate = np.ones((6,3))
g_steps = 20
@ -634,6 +635,7 @@ def gen_rotatey_path(standby_coordinate):
return path
def gen_rotatez_path(standby_coordinate):
# standby_coordinate = np.ones((6,3))
g_steps = 20
@ -651,7 +653,8 @@ def gen_rotatez_path(standby_coordinate):
x = xy_radius * np.cos(i*step_angle)
y = xy_radius * np.sin(i*step_angle)
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)
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)
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,
@ -694,5 +697,208 @@ def gen_rotatez_path(standby_coordinate):
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
def gen_twist_path(standby_coordinate):
g_steps = 20
raise_angle = 3
twist_x_angle = 20
twise_y_angle = 12
assert (g_steps % 4) == 0
quarter = int(g_steps / 4)
step_x_angle = twist_x_angle / quarter
step_y_angle = twise_y_angle / quarter
m = get_rotate_x_matrix(raise_angle)
path = np.zeros((g_steps, 6, 3))
return path
for i in range(quarter):
temp = m * get_rotate_z_matrix(i*step_x_angle) * \
get_rotate_x_matrix(i*step_y_angle)
idx = i+quarter*0
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):
temp = m * get_rotate_z_matrix((quarter-i)*step_x_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,
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):
temp = m * get_rotate_z_matrix(-i*step_x_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,
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):
temp = m * get_rotate_z_matrix((-quarter+i)*step_x_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,
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

Write Preview
Loading…
Cancel
Save