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 12:17:45 -05:00
parent e4e2d16924
commit deed6b596a
1 changed files with 20 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
software/raspberry pi/path_generator.py
View File

@ -76,9 +76,10 @@ def gen_fastwalk_path(standby_coordinate,
'type': 'motion'}
def gen_leftturn_path(standby_coordinate,
def gen_turn_path(standby_coordinate,
g_steps=20,
g_radius=25):
g_radius=25,
direction='left'):
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
@ -87,26 +88,14 @@ def gen_leftturn_path(standby_coordinate,
semi_circle = semicircle_generator(g_radius, g_steps)
mir_path = np.roll(semi_circle, halfsteps, axis=0)
if direction == 'left':
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,
g_steps=20,
g_radius=25):
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
semi_circle = semicircle_generator(g_radius, g_steps)
mir_path = np.roll(semi_circle, halfsteps, axis=0)
path = np.zeros((g_steps, 6, 3))
elif direction == 'right':
path[:, 0, :] = path_rotate_z(semi_circle, 45+180)
path[:, 1, :] = path_rotate_z(mir_path, 0+180)
path[:, 2, :] = path_rotate_z(semi_circle, 315+180)
@ -114,7 +103,8 @@ def gen_rightturn_path(standby_coordinate,
path[:, 4, :] = path_rotate_z(semi_circle, 180+180)
path[:, 5, :] = path_rotate_z(mir_path, 135+180)
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_shiftleft_path(standby_coordinate,