From f754e3e812c3d27065cee5faac5b7f6525ae24ed Mon Sep 17 00:00:00 2001 From: Zhengyu Peng Date: Fri, 10 Dec 2021 14:26:41 -0500 Subject: [PATCH] rotate z --- software/rpi/hexapod.py | 5 +-- software/rpi/path_generator.py | 63 ++++++++++++++++++++++++++++++++++ 2 files changed, 66 insertions(+), 2 deletions(-) diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py index 48e3152..7d6e03d 100644 --- a/software/rpi/hexapod.py +++ b/software/rpi/hexapod.py @@ -16,7 +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, gen_rotatey_path +from path_generator import gen_rotatex_path, gen_rotatey_path, gen_rotatez_path SIN30 = 0.5 @@ -104,6 +104,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.standby() time.sleep(1) @@ -137,7 +138,7 @@ class Hexapod: # self.move(self.rotatex_path, 0.005) for mm in range(0, 20): - self.move(self.rotatey_path, 0.005) + self.move(self.rotatez_path, 0.005) time.sleep(1) self.standby() diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py index feac06e..b2b89c0 100644 --- a/software/rpi/path_generator.py +++ b/software/rpi/path_generator.py @@ -632,4 +632,67 @@ def gen_rotatey_path(standby_coordinate): 1]*m[2, 1]+standby_coordinate[5, 2]*m[2, 2]+m[2, 3] # result.append(m) + return path + +def gen_rotatez_path(standby_coordinate): + # standby_coordinate = np.ones((6,3)) + g_steps = 20 + + z_lift = 4.5 + xy_radius = 1 + + assert (g_steps % 4) == 0 + + path = np.zeros((g_steps, 6, 3)) + + step_angle = 2*np.pi / g_steps + + for i in range(g_steps): + 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) + 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] + + return path \ No newline at end of file