master
Zhengyu Peng 3 years ago
parent 5ef8f52b41
commit d020a35a5d

@ -114,8 +114,8 @@ class Hexapod:
for mm in range(0, 20): for mm in range(0, 20):
self.move(self.forward_path, 0.005) self.move(self.forward_path, 0.005)
# for mm in range(0, 20): for mm in range(0, 20):
# self.move(self.backward_path, 0.005) self.move(self.backward_path, 0.005)
# for mm in range(0, 20): # for mm in range(0, 20):
# self.move(self.fastforward_path, 0.005) # self.move(self.fastforward_path, 0.005)

@ -17,10 +17,8 @@ def semicircle_generator(radius, steps, reverse=False):
# second half, move forward in semicircle shape (y, z change) # second half, move forward in semicircle shape (y, z change)
angle = np.pi - step_angle*halfsteps_array angle = np.pi - step_angle*halfsteps_array
y = radius * np.cos(angle) result[halfsteps:, 1] = radius * np.cos(angle)
z = radius * np.sin(angle) result[halfsteps:, 2] = radius * np.sin(angle)
result[halfsteps:, 1] = y
result[halfsteps:, 2] = z
result = np.roll(result, int(steps/4), axis=0) result = np.roll(result, int(steps/4), axis=0)
@ -43,14 +41,11 @@ def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False):
# first half, move backward (only y change) # first half, move backward (only y change)
result[:halfsteps, 1] = y_radius - halfsteps_array*y_radius*2/(halfsteps) result[:halfsteps, 1] = y_radius - halfsteps_array*y_radius*2/(halfsteps)
# second half, move forward in semicircle shape (y, z change) # second half, move forward in semicircle shape (x, y, z change)
angle = np.pi - step_angle*halfsteps_array angle = np.pi - step_angle*halfsteps_array
y = y_radius * np.cos(angle) result[halfsteps:, 0] = x_radius * np.sin(angle)
z = z_radius * np.sin(angle) result[halfsteps:, 1] = y_radius * np.cos(angle)
x = x_radius * np.sin(angle) result[halfsteps:, 2] = z_radius * np.sin(angle)
result[halfsteps:, 0] = x
result[halfsteps:, 1] = y
result[halfsteps:, 2] = z
result = np.roll(result, int(steps/4), axis=0) result = np.roll(result, int(steps/4), axis=0)

@ -6,21 +6,14 @@ from collections import deque
def gen_forward_path(standby_coordinate, g_steps=20, g_radius=25): def gen_forward_path(standby_coordinate, g_steps=20, g_radius=25):
assert (g_steps % 4) == 0
halfsteps = int(g_steps/2) halfsteps = int(g_steps/2)
g_steps = halfsteps*2
path = np.zeros((g_steps, 6, 3)) path = np.zeros((g_steps, 6, 3))
# path[:, 0, :] = semicircle_generator(g_radius, g_steps)
semi_circle = semicircle_generator(g_radius, g_steps) semi_circle = semicircle_generator(g_radius, g_steps)
# mir_path = np.roll(path[:, 0, :], halfsteps, axis=0)
# path[:, 2, :] = path[:, 0, :]
# path[:, 4, :] = path[:, 0, :]
# path[:, 1, :] = mir_path
# path[:, 3, :] = mir_path
# path[:, 5, :] = mir_path
path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1)) path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
path[:, [1, 3, 5], :] = np.tile( path[:, [1, 3, 5], :] = np.tile(
np.roll(semi_circle[:, np.newaxis, :], halfsteps, axis=0), (1, 3, 1)) np.roll(semi_circle[:, np.newaxis, :], halfsteps, axis=0), (1, 3, 1))
@ -28,22 +21,18 @@ def gen_forward_path(standby_coordinate, g_steps = 20, g_radius = 25):
return path+np.tile(standby_coordinate, (g_steps, 1, 1)) return path+np.tile(standby_coordinate, (g_steps, 1, 1))
def gen_backward_path(standby_coordinate): def gen_backward_path(standby_coordinate, g_steps=20, g_radius=25):
# assert (g_steps % 4) == 0 assert (g_steps % 4) == 0
g_steps = 20
g_radius = 25
halfsteps = int(g_steps/2) halfsteps = int(g_steps/2)
path = np.zeros((g_steps, 6, 3)) path = np.zeros((g_steps, 6, 3))
path[:, 0, :] = semicircle_generator(g_radius, g_steps, reverse=True) semi_circle = semicircle_generator(g_radius, g_steps, reverse=True)
mir_path = np.roll(path[:, 0, :], halfsteps, axis=0) path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
path[:, 2, :] = path[:, 0, :] path[:, [1, 3, 5], :] = np.tile(
path[:, 4, :] = path[:, 0, :] np.roll(semi_circle[:, np.newaxis, :], halfsteps, axis=0), (1, 3, 1))
path[:, 1, :] = mir_path
path[:, 3, :] = mir_path
path[:, 5, :] = mir_path
return path+np.tile(standby_coordinate, (g_steps, 1, 1)) return path+np.tile(standby_coordinate, (g_steps, 1, 1))

Loading…
Cancel
Save