update
This commit is contained in:
Zhengyu Peng
parent
c3192d33a1
commit
965e3acfd8
@ -40,7 +40,7 @@ import json
|
||||
from path_generator import gen_walk_path
|
||||
from path_generator import gen_fastwalk_path
|
||||
from path_generator import gen_turn_path
|
||||
from path_generator import gen_shift_path
|
||||
# from path_generator import gen_shift_path
|
||||
from path_generator import gen_climb_path
|
||||
from path_generator import gen_rotatex_path, gen_rotatey_path, gen_rotatez_path
|
||||
from path_generator import gen_twist_path
|
||||
@ -55,15 +55,20 @@ class Hexapod(Thread):
|
||||
CMD_STANDBY = 'standby'
|
||||
CMD_LAYDOWN = 'laydown'
|
||||
|
||||
CMD_FORWARD = 'forward'
|
||||
CMD_BACKWARD = 'backward'
|
||||
CMD_WALK_0 = 'walk0'
|
||||
CMD_WALK_180 = 'walk180'
|
||||
|
||||
CMD_WALK_R45 = 'walkr45'
|
||||
CMD_WALK_R90 = 'walkr90'
|
||||
CMD_WALK_R135 = 'walkr135'
|
||||
|
||||
CMD_WALK_L45 = 'walkl45'
|
||||
CMD_WALK_L90 = 'walkl90'
|
||||
CMD_WALK_L135 = 'walkl135'
|
||||
|
||||
CMD_FASTFORWARD = 'fastforward'
|
||||
CMD_FASTBACKWARD = 'fastbackward'
|
||||
|
||||
CMD_SHIFTLEFT = 'shiftleft'
|
||||
CMD_SHIFTRIGHT = 'shiftright'
|
||||
|
||||
CMD_TURNLEFT = 'turnleft'
|
||||
CMD_TURNRIGHT = 'turnright'
|
||||
|
||||
@ -153,10 +158,22 @@ class Hexapod(Thread):
|
||||
self.cmd_dict = {
|
||||
self.CMD_STANDBY: self.standby_posture,
|
||||
self.CMD_LAYDOWN: self.gen_posture(0, 15),
|
||||
self.CMD_FORWARD: gen_walk_path(
|
||||
self.standby_posture['coord']),
|
||||
self.CMD_BACKWARD: gen_walk_path(
|
||||
self.standby_posture['coord'], reverse=True),
|
||||
self.CMD_WALK_0: gen_walk_path(
|
||||
self.standby_posture['coord'], direction=0),
|
||||
self.CMD_WALK_180: gen_walk_path(
|
||||
self.standby_posture['coord'], direction=180),
|
||||
self.CMD_WALK_R45: gen_walk_path(
|
||||
self.standby_posture['coord'], direction=315),
|
||||
self.CMD_WALK_R90: gen_walk_path(
|
||||
self.standby_posture['coord'], direction=270),
|
||||
self.CMD_WALK_R135: gen_walk_path(
|
||||
self.standby_posture['coord'], direction=225),
|
||||
self.CMD_WALK_L45: gen_walk_path(
|
||||
self.standby_posture['coord'], direction=45),
|
||||
self.CMD_WALK_L90: gen_walk_path(
|
||||
self.standby_posture['coord'], direction=90),
|
||||
self.CMD_WALK_L135: gen_walk_path(
|
||||
self.standby_posture['coord'], direction=135),
|
||||
self.CMD_FASTFORWARD: gen_fastwalk_path(
|
||||
self.standby_posture['coord']),
|
||||
self.CMD_FASTBACKWARD: gen_fastwalk_path(
|
||||
@ -165,10 +182,6 @@ class Hexapod(Thread):
|
||||
self.standby_posture['coord'], direction='left'),
|
||||
self.CMD_TURNRIGHT: gen_turn_path(
|
||||
self.standby_posture['coord'], direction='right'),
|
||||
self.CMD_SHIFTLEFT: gen_shift_path(
|
||||
self.standby_posture['coord'], direction='left'),
|
||||
self.CMD_SHIFTRIGHT: gen_shift_path(
|
||||
self.standby_posture['coord'], direction='right'),
|
||||
self.CMD_CLIMBFORWARD: gen_climb_path(
|
||||
self.standby_posture['coord'], reverse=False),
|
||||
self.CMD_CLIMBBACKWARD: gen_climb_path(
|
||||
|
||||
@ -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,
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user