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-23 11:36:22 -05:00
parent 76849e70fc
commit 6e9ee7452b
1 changed files with 10 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
software/raspberry pi/path_generator.py
View File

@ -50,20 +50,21 @@ def gen_walk_path(standby_coordinate,
'type': 'motion'}
def gen_fastforward_path(standby_coordinate,
g_steps=20,
y_radius=50,
z_radius=30,
x_radius=10):
def gen_fastwalk_path(standby_coordinate,
g_steps=20,
y_radius=50,
z_radius=30,
x_radius=10,
reverse=False):
assert (g_steps % 2) == 0
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
semi_circle_r = semicircle2_generator(
g_steps, y_radius, z_radius, x_radius)
g_steps, y_radius, z_radius, x_radius, reverse=reverse)
semi_circle_l = semicircle2_generator(
g_steps, y_radius, z_radius, -x_radius)
g_steps, y_radius, z_radius, -x_radius, reverse=reverse)
path[:, [0, 2], :] = np.tile(semi_circle_r[:, np.newaxis, :], (1, 2, 1))
path[:, 1, :] = np.roll(semi_circle_r, halfsteps, axis=0)
@ -71,31 +72,8 @@ def gen_fastforward_path(standby_coordinate,
path[:, [3, 5], :] = np.tile(
np.roll(semi_circle_l[:, np.newaxis, :], halfsteps, axis=0), (1, 2, 1))
return path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_fastbackward_path(standby_coordinate,
g_steps=20,
y_radius=50,
z_radius=30,
x_radius=10):
assert (g_steps % 2) == 0
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
semi_circle_r = semicircle2_generator(
g_steps, y_radius, z_radius, x_radius, reverse=True)
semi_circle_l = semicircle2_generator(
g_steps, y_radius, z_radius, -x_radius, reverse=True)
path[:, [0, 2], :] = np.tile(semi_circle_r[:, np.newaxis, :], (1, 2, 1))
path[:, 1, :] = np.roll(semi_circle_r, halfsteps, axis=0)
path[:, 4, :] = semi_circle_l
path[:, [3, 5], :] = np.tile(
np.roll(semi_circle_l[:, np.newaxis, :], halfsteps, axis=0), (1, 2, 1))
return path+np.tile(standby_coordinate, (g_steps, 1, 1))
return {'coord': path+np.tile(standby_coordinate, (g_steps, 1, 1)),
'type': 'motion'}
def gen_leftturn_path(standby_coordinate,