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

climb

Browse Source
This commit is contained in:
Zhengyu Peng
2021-12-09 22:56:58 -05:00
parent 9640f03e2e
commit 578b390b72
2 changed files with 43 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
software/rpi/path_generator.py
View File

@ -176,3 +176,37 @@ def gen_shiftright_path():
shiftright[:, 5, :] = mir_path
return shiftright
def gen_climb_path():
g_steps = 20
y_radius = 20
z_radius = 80
x_radius = 30
z_shift = -30
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
rpath = semicircle2_generator(g_steps, y_radius, z_radius, x_radius)
rpath[:, 2] = rpath[:, 2]+z_shift
# rpath = [(x, y, z + z_shift) for x, y,
# z in semicircle2_generator(g_steps, y_radius, z_radius, x_radius)]
lpath = semicircle2_generator(g_steps, y_radius, z_radius, -x_radius)
lpath[:, 2] = lpath[:, 2]+z_shift
# lpath = [(x, y, z + z_shift) for x, y,
# z in semicircle2_generator(g_steps, y_radius, z_radius, -x_radius)]
mir_rpath = np.roll(rpath, halfsteps, axis=0)
mir_lpath = np.roll(lpath, halfsteps, axis=0)
climbpath = np.zeros((g_steps, 6, 3))
climbpath[:, 0, :] = rpath
climbpath[:, 1, :] = mir_rpath
climbpath[:, 2, :] = rpath
climbpath[:, 3, :] = mir_lpath
climbpath[:, 4, :] = lpath
climbpath[:, 5, :] = mir_lpath
return climbpath