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

Update path_generator.py

Browse Source
This commit is contained in:
Zhengyu Peng 2022-02-16 20:34:51 -05:00
parent 5dc36c60ca
commit 8c4b9b76ce
1 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
software/raspberry pi/path_generator.py
View File

@ -30,6 +30,24 @@ from lib import get_rotate_x_matrix, get_rotate_y_matrix, get_rotate_z_matrix
import numpy as np
def gen_standup_path(laydown_coordinate,
standby_coordinate,
g_steps=20):
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
semi_circle = semicircle_generator(g_radius, g_steps)
path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
path[:, [1, 3, 5], :] = np.tile(
np.roll(semi_circle[:, np.newaxis, :], halfsteps, axis=0), (1, 3, 1))
return path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_forward_path(standby_coordinate,
g_steps=20,
g_radius=25):