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 2021-12-10 16:29:43 -05:00
parent 264624bc84
commit 5ef8f52b41
1 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
software/rpi/path_generator.py
View File

@ -5,22 +5,25 @@ import numpy as np
from collections import deque
def gen_forward_path(standby_coordinate):
# assert (g_steps % 4) == 0
g_steps = 20
g_radius = 25
def gen_forward_path(standby_coordinate, g_steps = 20, g_radius = 25):
halfsteps = int(g_steps/2)
g_steps = halfsteps*2
path = np.zeros((g_steps, 6, 3))
path[:, 0, :] = semicircle_generator(g_radius, g_steps)
# path[:, 0, :] = semicircle_generator(g_radius, g_steps)
semi_circle = semicircle_generator(g_radius, g_steps)
mir_path = np.roll(path[:, 0, :], halfsteps, axis=0)
path[:, 2, :] = path[:, 0, :]
path[:, 4, :] = path[:, 0, :]
path[:, 1, :] = mir_path
path[:, 3, :] = mir_path
path[:, 5, :] = mir_path
# mir_path = np.roll(path[:, 0, :], halfsteps, axis=0)
# path[:, 2, :] = path[:, 0, :]
# path[:, 4, :] = path[:, 0, :]
# path[:, 1, :] = mir_path
# path[:, 3, :] = mir_path
# path[:, 5, :] = mir_path
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))