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

update

Browse Source
This commit is contained in:
Zhengyu Peng
2021-12-08 22:39:53 -05:00
parent da4512f71c
commit ed79951316
2 changed files with 177 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
software/rpi/hexapod.py
View File

@@ -12,6 +12,10 @@ import numpy as np
import time
import json
from collections import deque
from lib import semicircle_generator
SIN30 = 0.5
COS30 = 0.866
SIN45 = 0.7071
@@ -88,13 +92,52 @@ class Hexapod:
# self.leg_4.reset()
# self.leg_5.reset()
self.standby()
self.standby_coordinate = np.zeros((6, 3))
self.ik(self.standby_coordinate)
self.standby()
time.sleep(0.1)
# self.ik(self.standby_coordinate)
full_path = self.path_generator()
for idx in range(0, 20):
move_to = np.array([full_path[0][0][idx], full_path[0][1][idx], full_path[0][2][idx], full_path[0][3][idx], full_path[0][4][idx], full_path[0][5][idx]])+self.standby_coordinate
self.ik(move_to)
self.leg_0.set_angle(0, self.angles[0,0])
self.leg_0.set_angle(1, self.angles[0,1])
self.leg_0.set_angle(2, self.angles[0,2])
# self.leg_1.set_angle(0, self.angles[1,0])
# self.leg_1.set_angle(1, self.angles[1,1])
# self.leg_1.set_angle(2, self.angles[1,2])
# self.leg_2.set_angle(0, self.angles[2,0])
# self.leg_2.set_angle(1, self.angles[2,1])
# self.leg_2.set_angle(2, self.angles[2,2])
# self.leg_3.set_angle(0, self.angles[3,0])
# self.leg_3.set_angle(1, self.angles[3,1])
# self.leg_3.set_angle(2, self.angles[3,2])
# self.leg_4.set_angle(0, self.angles[4,0])
# self.leg_4.set_angle(1, self.angles[4,1])
# self.leg_4.set_angle(2, self.angles[4,2])
# self.leg_5.set_angle(0, self.angles[5,0])
# self.leg_5.set_angle(1, self.angles[5,1])
# self.leg_5.set_angle(2, self.angles[5,2])
time.sleep(0.1)
print(np.shape(move_to))
# print(np.array([full_path[0][0][0], full_path[0][1][0], full_path[0][2][0], full_path[0][3][0], full_path[0][4][0], full_path[0][5][0]]))
print(self.angles)
def standby(self):
self.standby_coordinate = np.zeros((6, 3))
self.standby_coordinate[:, 0] = np.array(self.mount_x)+(self.root_j1+self.j1_j2+(
self.j2_j3*COS30)+self.j3_tip*SIN15)*np.cos(self.mount_angle)
@@ -154,6 +197,19 @@ class Hexapod:
self.angles[:, 1] = 90-((ar + a1) * 180 / np.pi)
self.angles[:, 2] = (90 - ((a1 + a2) * 180 / np.pi))+90
def path_generator(self):
# assert (g_steps % 4) == 0
g_steps = 20
g_radius = 25
halfsteps = int(g_steps/2)
path = semicircle_generator(g_radius, g_steps)
mir_path = deque(path)
mir_path.rotate(halfsteps)
return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)
def main():

118
software/rpi/lib.py Normal file
View File

@@ -0,0 +1,118 @@
from collections import deque
import math
import numpy as np
pi = math.acos(-1)
def semicircle_generator(radius, steps, reverse=False):
assert (steps % 4) == 0
halfsteps = int(steps/2)
step_angle = pi / halfsteps
result = []
# first half, move backward (only y change)
for i in range(halfsteps):
result.append((0, radius - i*radius*2/(halfsteps), 0))
# second half, move forward in semicircle shape (y, z change)
for i in range(halfsteps):
angle = pi - step_angle*i
y = radius * math.cos(angle)
z = radius * math.sin(angle)
result.append((0, y, z))
result = deque(result)
result.rotate(int(steps/4))
if reverse:
result = deque(reversed(result))
result.rotate(1)
return result
def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False):
assert (steps % 4) == 0
halfsteps = int(steps/2)
step_angle = pi / halfsteps
result = []
# first half, move backward (only y change)
for i in range(halfsteps):
result.append((0, y_radius - i*y_radius*2/(halfsteps), 0))
# second half, move forward in semicircle shape (y, z change)
for i in range(halfsteps):
angle = pi - step_angle*i
y = y_radius * math.cos(angle)
z = z_radius * math.sin(angle)
x = x_radius * math.sin(angle)
result.append((x, y, z))
result = deque(result)
result.rotate(int(steps/4))
if reverse:
result = deque(reversed(result))
result.rotate(1)
return result
def get_rotate_x_matrix(angle):
angle = angle * pi / 180
return np.matrix([
[1, 0, 0, 0],
[0, math.cos(angle), -math.sin(angle), 0],
[0, math.sin(angle), math.cos(angle), 0],
[0, 0, 0, 1],
])
def get_rotate_y_matrix(angle):
angle = angle * pi / 180
return np.matrix([
[math.cos(angle), 0, math.sin(angle), 0],
[0, 1, 0, 0],
[-math.sin(angle), 0, math.cos(angle), 0],
[0, 0, 0, 1],
])
def get_rotate_z_matrix(angle):
angle = angle * pi / 180
return np.matrix([
[math.cos(angle), -math.sin(angle), 0, 0],
[math.sin(angle), math.cos(angle), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1],
])
def matrix_mul(m, pt):
ptx = list(pt) + [1]
return list((m * np.matrix(ptx).T).T.flat)[:-1]
def point_rotate_x(pt, angle):
ptx = list(pt) + [1]
return list((get_rotate_x_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
def point_rotate_y(pt, angle):
ptx = list(pt) + [1]
return list((get_rotate_y_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
def point_rotate_z(pt, angle):
ptx = list(pt) + [1]
return list((get_rotate_z_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
def path_rotate_x(path, angle):
return [point_rotate_x(p, angle) for p in path]
def path_rotate_y(path, angle):
return [point_rotate_y(p, angle) for p in path]
def path_rotate_z(path, angle):
return [point_rotate_z(p, angle) for p in path]
if __name__ == '__main__':
pt = [0, 1, 0]
print(point_rotate_z(pt, 270))