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

update

Browse Source
This commit is contained in:
Zhengyu Peng
2022-02-23 15:55:58 -05:00
parent 7dd9b5a490
commit 75bfa63a22
2 changed files with 59 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
software/raspberry pi/path_generator.py
View File

@ -30,21 +30,46 @@ from lib import get_rotate_x_matrix, get_rotate_y_matrix, get_rotate_z_matrix
import numpy as np
# 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, 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 {'coord': path+np.tile(standby_coordinate, (g_steps, 1, 1)),
# 'type': 'motion'}
def gen_walk_path(standby_coordinate,
g_steps=20,
g_radius=25,
reverse=False):
direction=0):
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
# if direction == 'left':
# shift_angle = 90
# elif direction == 'right':
# shift_angle = 270
semi_circle = semicircle_generator(g_radius, g_steps)
# shift 90 degree to make the path "left" shift
semi_circle = np.array(path_rotate_z(semi_circle, direction))
mir_path = np.roll(semi_circle, halfsteps, axis=0)
path = np.zeros((g_steps, 6, 3))
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))
path[:, [1, 3, 5], :] = np.tile(mir_path[:, np.newaxis, :], (1, 3, 1))
return {'coord': path+np.tile(standby_coordinate, (g_steps, 1, 1)),
'type': 'motion'}
@ -107,31 +132,6 @@ def gen_turn_path(standby_coordinate,
'type': 'motion'}
def gen_shift_path(standby_coordinate,
g_steps=20,
g_radius=25,
direction='left'):
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2)
if direction == 'left':
shift_angle = 90
elif direction == 'right':
shift_angle = 270
semi_circle = semicircle_generator(g_radius, g_steps)
# shift 90 degree to make the path "left" shift
semi_circle = np.array(path_rotate_z(semi_circle, shift_angle))
mir_path = np.roll(semi_circle, halfsteps, axis=0)
path = np.zeros((g_steps, 6, 3))
path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
path[:, [1, 3, 5], :] = np.tile(mir_path[:, np.newaxis, :], (1, 3, 1))
return {'coord': path+np.tile(standby_coordinate, (g_steps, 1, 1)),
'type': 'motion'}
def gen_climb_path(standby_coordinate,
g_steps=20,
y_radius=20,