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:31:01 -05:00
parent 36ca3b0f0a
commit 7f6651ede1
1 changed files with 7 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
software/raspberry pi/path_generator.py
View File

@@ -30,58 +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):
def gen_walk_path(standby_coordinate,
g_steps=20,
g_radius=25,
reverse=False):
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)
semi_circle = semicircle_generator(g_radius, g_steps, reverse=reverse)
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):
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_backward_path(standby_coordinate,
g_steps=20,
g_radius=25):
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, reverse=True)
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))
return {'coord': path+np.tile(standby_coordinate, (g_steps, 1, 1)),
'type': 'motion'}
def gen_fastforward_path(standby_coordinate,