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

fast walk

Browse Source
This commit is contained in:
Zhengyu Peng
2021-12-09 21:35:50 -05:00
parent e06c9c2979
commit 147356b1ae
3 changed files with 75 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
software/rpi/path_generator.py
View File

@ -1,4 +1,4 @@
from lib import semicircle_generator
from lib import semicircle_generator, semicircle2_generator
import numpy as np
@ -40,3 +40,53 @@ def gen_backward_path():
path[:, 5, :] = mir_path
return path
def gen_fastforward_path():
g_steps = 20
y_radius = 50
z_radius = 30
x_radius = 10
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
path[:, 0, :] = semicircle2_generator(
g_steps, y_radius, z_radius, x_radius)
path[:, 4, :] = semicircle2_generator(
g_steps, y_radius, z_radius, -x_radius)
mir_rpath = np.roll(path[:, 0, :], halfsteps, axis=0)
path[:, 1, :] = mir_rpath
path[:, 2, :] = path[:, 0, :]
mir_lpath = np.roll(path[:, 4, :], halfsteps, axis=0)
path[:, 3, :] = mir_lpath
path[:, 5, :] = mir_lpath
return path
def gen_fastbackward_path():
g_steps = 20
y_radius = 50
z_radius = 30
x_radius = 10
halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3))
path[:, 0, :] = semicircle2_generator(
g_steps, y_radius, z_radius, x_radius, reverse=True)
path[:, 4, :] = semicircle2_generator(
g_steps, y_radius, z_radius, -x_radius, reverse=True)
mir_rpath = np.roll(path[:, 0, :], halfsteps, axis=0)
path[:, 1, :] = mir_rpath
path[:, 2, :] = path[:, 0, :]
mir_lpath = np.roll(path[:, 4, :], halfsteps, axis=0)
path[:, 3, :] = mir_lpath
path[:, 5, :] = mir_lpath
return path