From 0edcd611095a8d9e6b200a53138566e5e824de2c Mon Sep 17 00:00:00 2001 From: Zhengyu Peng Date: Fri, 10 Dec 2021 15:03:00 -0500 Subject: [PATCH] twist --- software/rpi/hexapod.py | 4 +- software/rpi/path_generator.py | 212 ++++++++++++++++++++++++++++++++- 2 files changed, 212 insertions(+), 4 deletions(-) diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py index 7d6e03d..aca37ec 100644 --- a/software/rpi/hexapod.py +++ b/software/rpi/hexapod.py @@ -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() diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py index b2b89c0..ba63746 100644 --- a/software/rpi/path_generator.py +++ b/software/rpi/path_generator.py @@ -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 \ No newline at end of file + 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