shark
/
hexapod
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

use numpy array

Browse Source
master
Zhengyu Peng 3 years ago
parent 922eb50b72
commit bb61c12434
2 changed files with 42 additions and 36 deletions
Show Stats Download Patch File Download Diff File

55
software/rpi/hexapod.py
Unescape Escape View File

@ -103,33 +103,33 @@ class Hexapod:
for mm in range(0, 30): for mm in range(0, 30):
for idx in range(0, 20): for idx in range(0, 20):
move_to = np.array([full_path[0][0][idx], full_path[0][1][idx], full_path[0][2][idx], full_path[0][3][idx], full_path[0][4][idx], full_path[0][5][idx]])+self.standby_coordinate move_to = full_path[idx, :, :]+self.standby_coordinate
self.ik(move_to) self.ik(move_to)
self.leg_0.set_angle(0, self.angles[0,0]) self.leg_0.set_angle(0, self.angles[0, 0])
self.leg_0.set_angle(1, self.angles[0,1]) self.leg_0.set_angle(1, self.angles[0, 1])
self.leg_0.set_angle(2, self.angles[0,2]) self.leg_0.set_angle(2, self.angles[0, 2])
self.leg_1.set_angle(0, self.angles[1,0]) self.leg_1.set_angle(0, self.angles[1, 0])
self.leg_1.set_angle(1, self.angles[1,1]) self.leg_1.set_angle(1, self.angles[1, 1])
self.leg_1.set_angle(2, self.angles[1,2]) self.leg_1.set_angle(2, self.angles[1, 2])
self.leg_2.set_angle(0, self.angles[2,0]) self.leg_2.set_angle(0, self.angles[2, 0])
self.leg_2.set_angle(1, self.angles[2,1]) self.leg_2.set_angle(1, self.angles[2, 1])
self.leg_2.set_angle(2, self.angles[2,2]) self.leg_2.set_angle(2, self.angles[2, 2])
self.leg_3.set_angle(0, self.angles[3,0]) self.leg_3.set_angle(0, self.angles[3, 0])
self.leg_3.set_angle(1, self.angles[3,1]) self.leg_3.set_angle(1, self.angles[3, 1])
self.leg_3.set_angle(2, self.angles[3,2]) self.leg_3.set_angle(2, self.angles[3, 2])
self.leg_4.set_angle(0, self.angles[4,0]) self.leg_4.set_angle(0, self.angles[4, 0])
self.leg_4.set_angle(1, self.angles[4,1]) self.leg_4.set_angle(1, self.angles[4, 1])
self.leg_4.set_angle(2, self.angles[4,2]) self.leg_4.set_angle(2, self.angles[4, 2])
self.leg_5.set_angle(0, self.angles[5,0]) self.leg_5.set_angle(0, self.angles[5, 0])
self.leg_5.set_angle(1, self.angles[5,1]) self.leg_5.set_angle(1, self.angles[5, 1])
self.leg_5.set_angle(2, self.angles[5,2]) self.leg_5.set_angle(2, self.angles[5, 2])
time.sleep(0.005) time.sleep(0.005)
@ -138,7 +138,6 @@ class Hexapod:
print(self.angles) print(self.angles)
def standby(self): def standby(self):
self.standby_coordinate[:, 0] = np.array(self.mount_x)+(self.root_j1+self.j1_j2+( self.standby_coordinate[:, 0] = np.array(self.mount_x)+(self.root_j1+self.j1_j2+(
self.j2_j3*COS30)+self.j3_tip*SIN15)*np.cos(self.mount_angle) self.j2_j3*COS30)+self.j3_tip*SIN15)*np.cos(self.mount_angle)
@ -204,12 +203,20 @@ class Hexapod:
g_radius = 25 g_radius = 25
halfsteps = int(g_steps/2) halfsteps = int(g_steps/2)
path = semicircle_generator(g_radius, g_steps) path = np.zeros((g_steps, 6, 3))
mir_path = deque(path) path[:, 0, :] = semicircle_generator(g_radius, g_steps)
mir_path.rotate(halfsteps)
return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps) 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
return path
# return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)
def main(): def main():

23
software/rpi/lib.py
Unescape Escape View File

@ -9,25 +9,24 @@ def semicircle_generator(radius, steps, reverse=False):
step_angle = np.pi / halfsteps step_angle = np.pi / halfsteps
result = [] result = np.zeros((steps, 3))
halfsteps_array = np.arange(halfsteps)
# first half, move backward (only y change) # first half, move backward (only y change)
for i in range(halfsteps): result[:halfsteps, 1] = radius - halfsteps_array*radius*2/(halfsteps)
result.append((0, radius - i*radius*2/(halfsteps), 0))
# second half, move forward in semicircle shape (y, z change) # second half, move forward in semicircle shape (y, z change)
for i in range(halfsteps): angle = np.pi - step_angle*halfsteps_array
angle = np.pi - step_angle*i y = radius * np.cos(angle)
y = radius * math.cos(angle) z = radius * np.sin(angle)
z = radius * math.sin(angle) result[halfsteps:, 1] = y
result.append((0, y, z)) result[halfsteps:, 2] = z
result = deque(result) result = np.roll(result, int(steps/4), axis=0)
result.rotate(int(steps/4))
if reverse: if reverse:
result = deque(reversed(result)) result = np.flip(result, axis=0)
result.rotate(1) result = np.roll(result, 1, axis=0)
return result return result

Write Preview
Loading…
Cancel
Save