rotate x
This commit is contained in:
parent
c6b9405b27
commit
e6b0bee20d
@ -16,6 +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
|
||||
|
||||
|
||||
SIN30 = 0.5
|
||||
@ -101,6 +102,8 @@ class Hexapod:
|
||||
|
||||
self.climb_path = gen_climb_path()
|
||||
|
||||
self.rotatex_path = gen_rotatex_path(self.standby_coordinate)
|
||||
|
||||
self.standby()
|
||||
time.sleep(1)
|
||||
|
||||
@ -128,8 +131,11 @@ class Hexapod:
|
||||
# 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.climb_path, 0.005)
|
||||
|
||||
|
||||
self.move(self.rotatex_path, 0.005)
|
||||
|
||||
time.sleep(1)
|
||||
self.standby()
|
||||
@ -150,7 +156,8 @@ class Hexapod:
|
||||
|
||||
def move(self, path, interval):
|
||||
for p_idx in range(0, np.shape(path)[0]):
|
||||
dest = path[p_idx, :, :]+self.standby_coordinate
|
||||
# dest = path[p_idx, :, :]+self.standby_coordinate
|
||||
dest = path[p_idx, :, :]
|
||||
angles = self.inverse_kinematics(dest)
|
||||
|
||||
self.leg_0.move_junctions(angles[0, :])
|
||||
|
@ -1,5 +1,6 @@
|
||||
from lib import semicircle_generator, semicircle2_generator
|
||||
from lib import path_rotate_z
|
||||
from lib import get_rotate_x_matrix
|
||||
import numpy as np
|
||||
from collections import deque
|
||||
|
||||
@ -210,3 +211,215 @@ def gen_climb_path():
|
||||
climbpath[:, 5, :] = mir_lpath
|
||||
|
||||
return climbpath
|
||||
|
||||
|
||||
def gen_rotatex_path(standby_coordinate):
|
||||
# standby_coordinate = np.ones((6,3))
|
||||
g_steps = 20
|
||||
|
||||
swing_angle = 15
|
||||
y_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 = y_radius / quarter
|
||||
|
||||
for i in range(quarter):
|
||||
m = get_rotate_x_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_x_matrix(-i*step_angle)
|
||||
m[1, 3] = -y_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_x_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_x_matrix(i*step_angle)
|
||||
m[1, 3] = y_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
|
||||
# return result, "matrix", 50, (0, quarter*2)
|
||||
|
Loading…
x
Reference in New Issue
Block a user