From 43dad0fcfa4c8722366ec0babf1d6e0c6d908d25 Mon Sep 17 00:00:00 2001 From: Zhengyu Peng Date: Fri, 10 Dec 2021 22:10:00 -0500 Subject: [PATCH] use matrix calculation --- software/rpi/lib.py | 38 +++++++++++---------------------- software/rpi/path_generator.py | 39 +++++++++++++++++----------------- 2 files changed, 32 insertions(+), 45 deletions(-) diff --git a/software/rpi/lib.py b/software/rpi/lib.py index fd0401e..9c8bcd8 100644 --- a/software/rpi/lib.py +++ b/software/rpi/lib.py @@ -1,5 +1,3 @@ -from collections import deque -import math import numpy as np @@ -60,8 +58,8 @@ def get_rotate_x_matrix(angle): angle = angle * np.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, np.cos(angle), -np.sin(angle), 0], + [0, np.sin(angle), np.cos(angle), 0], [0, 0, 0, 1], ]) @@ -69,9 +67,9 @@ def get_rotate_x_matrix(angle): def get_rotate_y_matrix(angle): angle = angle * np.pi / 180 return np.matrix([ - [math.cos(angle), 0, math.sin(angle), 0], + [np.cos(angle), 0, np.sin(angle), 0], [0, 1, 0, 0], - [-math.sin(angle), 0, math.cos(angle), 0], + [-np.sin(angle), 0, np.cos(angle), 0], [0, 0, 0, 1], ]) @@ -79,8 +77,8 @@ def get_rotate_y_matrix(angle): def get_rotate_z_matrix(angle): angle = angle * np.pi / 180 return np.matrix([ - [math.cos(angle), -math.sin(angle), 0, 0], - [math.sin(angle), math.cos(angle), 0, 0], + [np.cos(angle), -np.sin(angle), 0, 0], + [np.sin(angle), np.cos(angle), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1], ]) @@ -91,31 +89,19 @@ def matrix_mul(m, pt): 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] + ptx = np.append(path, np.ones((np.shape(path)[0], 1)), axis=1) + return ((get_rotate_x_matrix(angle) * np.matrix(ptx).T).T)[:, :-1] def path_rotate_y(path, angle): - return [point_rotate_y(p, angle) for p in path] + ptx = np.append(path, np.ones((np.shape(path)[0], 1)), axis=1) + return ((get_rotate_y_matrix(angle) * np.matrix(ptx).T).T)[:, :-1] def path_rotate_z(path, angle): - return [point_rotate_z(p, angle) for p in path] + ptx = np.append(path, np.ones((np.shape(path)[0], 1)), axis=1) + return ((get_rotate_z_matrix(angle) * np.matrix(ptx).T).T)[:, :-1] if __name__ == '__main__': diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py index e78be67..2fc1a19 100644 --- a/software/rpi/path_generator.py +++ b/software/rpi/path_generator.py @@ -89,24 +89,25 @@ def gen_fastbackward_path(standby_coordinate, return path+np.tile(standby_coordinate, (g_steps, 1, 1)) -def gen_leftturn_path(standby_coordinate): - g_steps = 20 - g_radius = 25 +def gen_leftturn_path(standby_coordinate, + g_steps = 20, + g_radius = 25): assert (g_steps % 4) == 0 halfsteps = int(g_steps/2) - path = semicircle_generator(g_radius, g_steps) - mir_path = np.roll(path, halfsteps, axis=0) + path = np.zeros((g_steps, 6, 3)) + + semi_circle = semicircle_generator(g_radius, g_steps) + mir_path = np.roll(semi_circle, halfsteps, axis=0) - leftturn = np.zeros((g_steps, 6, 3)) - leftturn[:, 0, :] = np.array(path_rotate_z(path, 45)) - leftturn[:, 1, :] = np.array(path_rotate_z(mir_path, 0)) - leftturn[:, 2, :] = np.array(path_rotate_z(path, 315)) - leftturn[:, 3, :] = np.array(path_rotate_z(mir_path, 225)) - leftturn[:, 4, :] = np.array(path_rotate_z(path, 180)) - leftturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135)) + path[:, 0, :] = path_rotate_z(semi_circle, 45) + path[:, 1, :] = path_rotate_z(mir_path, 0) + path[:, 2, :] = path_rotate_z(semi_circle, 315) + path[:, 3, :] = path_rotate_z(mir_path, 225) + path[:, 4, :] = path_rotate_z(semi_circle, 180) + path[:, 5, :] = path_rotate_z(mir_path, 135) - return leftturn+np.tile(standby_coordinate, (g_steps, 1, 1)) + return path+np.tile(standby_coordinate, (g_steps, 1, 1)) def gen_rightturn_path(standby_coordinate): @@ -119,12 +120,12 @@ def gen_rightturn_path(standby_coordinate): mir_path = np.roll(path, halfsteps, axis=0) rightturn = np.zeros((g_steps, 6, 3)) - rightturn[:, 0, :] = np.array(path_rotate_z(path, 45+180)) - rightturn[:, 1, :] = np.array(path_rotate_z(mir_path, 0+180)) - rightturn[:, 2, :] = np.array(path_rotate_z(path, 315+180)) - rightturn[:, 3, :] = np.array(path_rotate_z(mir_path, 225+180)) - rightturn[:, 4, :] = np.array(path_rotate_z(path, 180+180)) - rightturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135+180)) + rightturn[:, 0, :] = path_rotate_z(path, 45+180) + rightturn[:, 1, :] = path_rotate_z(mir_path, 0+180) + rightturn[:, 2, :] = path_rotate_z(path, 315+180) + rightturn[:, 3, :] = path_rotate_z(mir_path, 225+180) + rightturn[:, 4, :] = path_rotate_z(path, 180+180) + rightturn[:, 5, :] = path_rotate_z(mir_path, 135+180) return rightturn+np.tile(standby_coordinate, (g_steps, 1, 1))