From 965e3acfd86b149addeffcc3f26c7cd2addd1b3f Mon Sep 17 00:00:00 2001 From: Zhengyu Peng Date: Wed, 23 Feb 2022 15:55:58 -0500 Subject: [PATCH] update --- software/raspberry pi/hexapod.py | 41 ++++++++++------ software/raspberry pi/path_generator.py | 62 ++++++++++++------------- 2 files changed, 58 insertions(+), 45 deletions(-) diff --git a/software/raspberry pi/hexapod.py b/software/raspberry pi/hexapod.py index cef16a4..82ba740 100644 --- a/software/raspberry pi/hexapod.py +++ b/software/raspberry pi/hexapod.py @@ -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( diff --git a/software/raspberry pi/path_generator.py b/software/raspberry pi/path_generator.py index a742677..a1d6d52 100644 --- a/software/raspberry pi/path_generator.py +++ b/software/raspberry pi/path_generator.py @@ -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) - path = np.zeros((g_steps, 6, 3)) + # if direction == 'left': + # shift_angle = 90 + # elif direction == 'right': + # shift_angle = 270 - semi_circle = semicircle_generator(g_radius, g_steps, reverse=reverse) + 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)) 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,