From 3b0bb5219b5d12c9a952994c41f6f9a4e7de0697 Mon Sep 17 00:00:00 2001 From: Zhengyu Peng Date: Fri, 10 Dec 2021 23:07:33 -0500 Subject: [PATCH] use matrix to simplify calculation --- software/rpi/path_generator.py | 187 ++------------------------------- 1 file changed, 7 insertions(+), 180 deletions(-) diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py index c665eca..2747cb2 100644 --- a/software/rpi/path_generator.py +++ b/software/rpi/path_generator.py @@ -204,204 +204,36 @@ def gen_rotatex_path(standby_coordinate, path = np.zeros((g_steps, 6, 3)) - result = [] step_angle = swing_angle / quarter step_offset = y_radius / quarter + scx = np.append(standby_coordinate, np.ones((6, 1)), axis=1) + for i in range(quarter): m = get_rotate_x_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) + path[i,:,:] = ((np.matmul(m, scx.T)).T)[:,:-1] + for i in range(quarter): m = get_rotate_x_matrix(-i*step_angle) m[1, 3] = -y_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) + path[i+quarter,:,:] = ((np.matmul(m, scx.T)).T)[:,:-1] for i in range(quarter): m = get_rotate_x_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) + path[i+quarter*2,:,:] = ((np.matmul(m, scx.T)).T)[:,:-1] for i in range(quarter): m = get_rotate_x_matrix(i*step_angle) m[1, 3] = y_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) + path[i+quarter*3,:,:] = ((np.matmul(m, scx.T)).T)[:,:-1] return path - # return result, "matrix", 50, (0, quarter*2) def gen_rotatey_path(standby_coordinate): @@ -416,7 +248,6 @@ def gen_rotatey_path(standby_coordinate): path = np.zeros((g_steps, 6, 3)) - result = [] step_angle = swing_angle / quarter step_offset = x_radius / quarter @@ -465,7 +296,6 @@ def gen_rotatey_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] - # result.append(m) for i in range(quarter): m = get_rotate_y_matrix(-i*step_angle) @@ -514,7 +344,6 @@ def gen_rotatey_path(standby_coordinate): 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) @@ -562,7 +391,6 @@ def gen_rotatey_path(standby_coordinate): 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) @@ -610,7 +438,6 @@ def gen_rotatey_path(standby_coordinate): 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