From 063f73588e09999645ba5337733a8da5705c7c00 Mon Sep 17 00:00:00 2001 From: Zhengyu Peng Date: Fri, 10 Dec 2021 13:40:19 -0500 Subject: [PATCH] rotate y --- software/rpi/hexapod.py | 8 +- software/rpi/path_generator.py | 320 +++++++++++++++++++++++++++------ 2 files changed, 270 insertions(+), 58 deletions(-) diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py index d9539a4..48e3152 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 +from path_generator import gen_rotatex_path, gen_rotatey_path SIN30 = 0.5 @@ -103,6 +103,7 @@ class Hexapod: self.climb_path = gen_climb_path() self.rotatex_path = gen_rotatex_path(self.standby_coordinate) + self.rotatey_path = gen_rotatey_path(self.standby_coordinate) self.standby() time.sleep(1) @@ -134,8 +135,9 @@ class Hexapod: # for mm in range(0, 20): # self.move(self.climb_path, 0.005) - - self.move(self.rotatex_path, 0.005) + # self.move(self.rotatex_path, 0.005) + for mm in range(0, 20): + self.move(self.rotatey_path, 0.005) time.sleep(1) self.standby() diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py index 9c2fdf8..feac06e 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 +from lib import get_rotate_x_matrix, get_rotate_y_matrix import numpy as np from collections import deque @@ -283,46 +283,46 @@ def gen_rotatex_path(standby_coordinate): 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 1]*m[2, 1]+standby_coordinate[5, 2]*m[2, 2]+m[2, 3] result.append(m) for i in range(quarter): @@ -331,46 +331,46 @@ def gen_rotatex_path(standby_coordinate): 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 1]*m[2, 1]+standby_coordinate[5, 2]*m[2, 2]+m[2, 3] result.append(m) for i in range(quarter): @@ -379,47 +379,257 @@ def gen_rotatex_path(standby_coordinate): 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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] + 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) + +def gen_rotatey_path(standby_coordinate): + # standby_coordinate = np.ones((6,3)) + g_steps = 20 + + swing_angle = 15 + x_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 = x_radius / quarter + + for i in range(quarter): + m = get_rotate_y_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_y_matrix(-i*step_angle) + m[1, 3] = -x_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_y_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_y_matrix(i*step_angle) + m[1, 3] = x_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 \ No newline at end of file