diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py index 8628be1..d31dd54 100644 --- a/software/rpi/hexapod.py +++ b/software/rpi/hexapod.py @@ -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_coordinate = np.zeros((6, 3)) + 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.ik(self.standby_coordinate) + # 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(): diff --git a/software/rpi/lib.py b/software/rpi/lib.py new file mode 100644 index 0000000..6d22749 --- /dev/null +++ b/software/rpi/lib.py @@ -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)) \ No newline at end of file