From d4dc987b553d1caf85e715665ae5242b5688ed31 Mon Sep 17 00:00:00 2001 From: Zhengyu Peng Date: Thu, 9 Dec 2021 21:35:50 -0500 Subject: [PATCH] fast walk --- software/rpi/hexapod.py | 13 +++++++-- software/rpi/lib.py | 26 ++++++++--------- software/rpi/path_generator.py | 52 +++++++++++++++++++++++++++++++++- 3 files changed, 75 insertions(+), 16 deletions(-) diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py index c6ed8db..df9e346 100644 --- a/software/rpi/hexapod.py +++ b/software/rpi/hexapod.py @@ -12,6 +12,7 @@ import numpy as np import time import json from path_generator import gen_forward_path, gen_backward_path +from path_generator import gen_fastforward_path, gen_fastbackward_path SIN30 = 0.5 @@ -86,16 +87,24 @@ class Hexapod: self.standby_coordinate = self.calculate_standby_coordinate(60, 75) self.forward_path = gen_forward_path() self.backward_path = gen_backward_path() + self.fastforward_path = gen_fastforward_path() + self.fastbackward_path = gen_fastbackward_path() self.standby() time.sleep(1) - for mm in range(0, 30): + for mm in range(0, 20): self.move(self.forward_path, 0.005) - for mm in range(0, 30): + for mm in range(0, 20): self.move(self.backward_path, 0.005) + for mm in range(0, 20): + self.move(self.fastforward_path, 0.005) + + for mm in range(0, 20): + self.move(self.fastbackward_path, 0.005) + def calculate_standby_coordinate(self, j2_angle, j3_angle): j2_rad = j2_angle/180*np.pi j3_rad = j3_angle/180*np.pi diff --git a/software/rpi/lib.py b/software/rpi/lib.py index 47e08e6..e132616 100644 --- a/software/rpi/lib.py +++ b/software/rpi/lib.py @@ -37,26 +37,26 @@ def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False): step_angle = np.pi / halfsteps - result = [] + result = np.zeros((steps, 3)) + halfsteps_array = np.arange(halfsteps) # first half, move backward (only y change) - for i in range(halfsteps): - result.append((0, y_radius - i*y_radius*2/(halfsteps), 0)) + result[:halfsteps, 1] = y_radius - halfsteps_array*y_radius*2/(halfsteps) # second half, move forward in semicircle shape (y, z change) - for i in range(halfsteps): - angle = np.pi - step_angle*i - y = y_radius * math.cos(angle) - z = z_radius * math.sin(angle) - x = x_radius * math.sin(angle) - result.append((x, y, z)) + angle = np.pi - step_angle*halfsteps_array + y = y_radius * np.cos(angle) + z = z_radius * np.sin(angle) + x = x_radius * np.sin(angle) + result[halfsteps:, 0] = x + result[halfsteps:, 1] = y + result[halfsteps:, 2] = z - result = deque(result) - result.rotate(int(steps/4)) + result = np.roll(result, int(steps/4), axis=0) if reverse: - result = deque(reversed(result)) - result.rotate(1) + result = np.flip(result, axis=0) + result = np.roll(result, 1, axis=0) return result diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py index 640c0d5..9f7d63e 100644 --- a/software/rpi/path_generator.py +++ b/software/rpi/path_generator.py @@ -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