fast walk

master
Zhengyu Peng 3 years ago
parent 653da35be9
commit d4dc987b55

@ -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

@ -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

@ -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

Loading…
Cancel
Save