shark/hexapod
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

use matrix calculation

Browse Source
This commit is contained in:
Zhengyu Peng 2021-12-10 22:10:00 -05:00
parent 053ab6ff4e
commit 43dad0fcfa
2 changed files with 32 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
software/rpi/lib.py
View File

@ -1,5 +1,3 @@
from collections import deque
import math
import numpy as np import numpy as np
@ -60,8 +58,8 @@ def get_rotate_x_matrix(angle):
angle = angle * np.pi / 180 angle = angle * np.pi / 180
return np.matrix([ return np.matrix([
[1, 0, 0, 0], [1, 0, 0, 0],
[0, math.cos(angle), -math.sin(angle), 0], [0, np.cos(angle), -np.sin(angle), 0],
[0, math.sin(angle), math.cos(angle), 0], [0, np.sin(angle), np.cos(angle), 0],
[0, 0, 0, 1], [0, 0, 0, 1],
]) ])
@ -69,9 +67,9 @@ def get_rotate_x_matrix(angle):
def get_rotate_y_matrix(angle): def get_rotate_y_matrix(angle):
angle = angle * np.pi / 180 angle = angle * np.pi / 180
return np.matrix([ return np.matrix([
[math.cos(angle), 0, math.sin(angle), 0], [np.cos(angle), 0, np.sin(angle), 0],
[0, 1, 0, 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], [0, 0, 0, 1],
]) ])
@ -79,8 +77,8 @@ def get_rotate_y_matrix(angle):
def get_rotate_z_matrix(angle): def get_rotate_z_matrix(angle):
angle = angle * np.pi / 180 angle = angle * np.pi / 180
return np.matrix([ return np.matrix([
[math.cos(angle), -math.sin(angle), 0, 0], [np.cos(angle), -np.sin(angle), 0, 0],
[math.sin(angle), math.cos(angle), 0, 0], [np.sin(angle), np.cos(angle), 0, 0],
[0, 0, 1, 0], [0, 0, 1, 0],
[0, 0, 0, 1], [0, 0, 0, 1],
]) ])
@ -91,31 +89,19 @@ def matrix_mul(m, pt):
return list((m * np.matrix(ptx).T).T.flat)[:-1] 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): 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): 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): 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__': if __name__ == '__main__':

39
software/rpi/path_generator.py
View File

@ -89,24 +89,25 @@ def gen_fastbackward_path(standby_coordinate,
return path+np.tile(standby_coordinate, (g_steps, 1, 1)) return path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_leftturn_path(standby_coordinate): def gen_leftturn_path(standby_coordinate,
g_steps = 20 g_steps = 20,
g_radius = 25 g_radius = 25):
assert (g_steps % 4) == 0 assert (g_steps % 4) == 0
halfsteps = int(g_steps/2) halfsteps = int(g_steps/2)
path = semicircle_generator(g_radius, g_steps) path = np.zeros((g_steps, 6, 3))
mir_path = np.roll(path, halfsteps, axis=0)
leftturn = np.zeros((g_steps, 6, 3)) semi_circle = semicircle_generator(g_radius, g_steps)
leftturn[:, 0, :] = np.array(path_rotate_z(path, 45)) mir_path = np.roll(semi_circle, halfsteps, axis=0)
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))
return leftturn+np.tile(standby_coordinate, (g_steps, 1, 1)) 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 path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_rightturn_path(standby_coordinate): def gen_rightturn_path(standby_coordinate):
@ -119,12 +120,12 @@ def gen_rightturn_path(standby_coordinate):
mir_path = np.roll(path, halfsteps, axis=0) mir_path = np.roll(path, halfsteps, axis=0)
rightturn = np.zeros((g_steps, 6, 3)) rightturn = np.zeros((g_steps, 6, 3))
rightturn[:, 0, :] = np.array(path_rotate_z(path, 45+180)) rightturn[:, 0, :] = path_rotate_z(path, 45+180)
rightturn[:, 1, :] = np.array(path_rotate_z(mir_path, 0+180)) rightturn[:, 1, :] = path_rotate_z(mir_path, 0+180)
rightturn[:, 2, :] = np.array(path_rotate_z(path, 315+180)) rightturn[:, 2, :] = path_rotate_z(path, 315+180)
rightturn[:, 3, :] = np.array(path_rotate_z(mir_path, 225+180)) rightturn[:, 3, :] = path_rotate_z(mir_path, 225+180)
rightturn[:, 4, :] = np.array(path_rotate_z(path, 180+180)) rightturn[:, 4, :] = path_rotate_z(path, 180+180)
rightturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135+180)) rightturn[:, 5, :] = path_rotate_z(mir_path, 135+180)
return rightturn+np.tile(standby_coordinate, (g_steps, 1, 1)) return rightturn+np.tile(standby_coordinate, (g_steps, 1, 1))