update

software/rpi/hexapod.py

@@ -12,6 +12,10 @@ import numpy as np
 import time
 import json
 
+from collections import deque
+
+from lib import semicircle_generator
+
 SIN30 = 0.5
 COS30 = 0.866
 SIN45 = 0.7071
@@ -88,13 +92,52 @@ class Hexapod:
         # self.leg_4.reset()
         # self.leg_5.reset()
 
-        self.standby()
+        self.standby_coordinate = np.zeros((6, 3))
 
-        self.ik(self.standby_coordinate)
+        self.standby()
+        time.sleep(0.1)
+
+        # self.ik(self.standby_coordinate)
+
+        full_path = self.path_generator()
+
+        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
+
+            self.ik(move_to)
+
+            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(2, self.angles[0,2])
+
+            # 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(2, self.angles[1,2])
+
+            # 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(2, self.angles[2,2])
+
+            # 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(2, self.angles[3,2])
+
+            # 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(2, self.angles[4,2])
+
+            # 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(2, self.angles[5,2])
+
+            time.sleep(0.1)
+
+        print(np.shape(move_to))
+        # print(np.array([full_path[0][0][0], full_path[0][1][0], full_path[0][2][0], full_path[0][3][0], full_path[0][4][0], full_path[0][5][0]]))
         print(self.angles)
 
     def standby(self):
-        self.standby_coordinate = np.zeros((6, 3))
+        
 
         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)
@@ -154,6 +197,19 @@ class Hexapod:
         self.angles[:, 1] = 90-((ar + a1) * 180 / np.pi)
         self.angles[:, 2] = (90 - ((a1 + a2) * 180 / np.pi))+90
 
+    def path_generator(self):
+        # assert (g_steps % 4) == 0
+        g_steps = 20
+        g_radius = 25
+        halfsteps = int(g_steps/2)
+
+        path = semicircle_generator(g_radius, g_steps)
+
+        mir_path = deque(path)
+        mir_path.rotate(halfsteps)
+
+        return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)
+
 
 def main():
 

software/rpi/lib.py

@@ -0,0 +1,118 @@
+from collections import deque
+import math
+import numpy as np
+
+pi = math.acos(-1)
+
+def semicircle_generator(radius, steps, reverse=False):
+    assert (steps % 4) == 0
+    halfsteps = int(steps/2)
+
+    step_angle = pi / halfsteps
+
+    result = []
+
+    # first half, move backward (only y change)
+    for i in range(halfsteps):
+        result.append((0, radius - i*radius*2/(halfsteps), 0))
+
+    # second half, move forward in semicircle shape (y, z change)
+    for i in range(halfsteps):
+        angle = pi - step_angle*i
+        y = radius * math.cos(angle)
+        z = radius * math.sin(angle)
+        result.append((0, y, z))
+
+    result = deque(result)
+    result.rotate(int(steps/4))
+
+    if reverse:
+        result = deque(reversed(result))
+        result.rotate(1)
+
+    return result
+
+def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False):
+    assert (steps % 4) == 0
+    halfsteps = int(steps/2)
+
+    step_angle = pi / halfsteps
+
+    result = []
+
+    # first half, move backward (only y change)
+    for i in range(halfsteps):
+        result.append((0, y_radius - i*y_radius*2/(halfsteps), 0))
+
+    # second half, move forward in semicircle shape (y, z change)
+    for i in range(halfsteps):
+        angle = 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))
+
+    result = deque(result)
+    result.rotate(int(steps/4))
+
+    if reverse:
+        result = deque(reversed(result))
+        result.rotate(1)
+
+    return result
+
+def get_rotate_x_matrix(angle):
+    angle = angle * pi / 180
+    return np.matrix([
+        [1, 0, 0, 0],
+        [0, math.cos(angle), -math.sin(angle), 0],
+        [0, math.sin(angle), math.cos(angle), 0],
+        [0, 0, 0, 1],
+    ])
+
+def get_rotate_y_matrix(angle):
+    angle = angle * pi / 180
+    return np.matrix([
+        [math.cos(angle), 0, math.sin(angle), 0],
+        [0, 1, 0, 0],
+        [-math.sin(angle), 0, math.cos(angle), 0],
+        [0, 0, 0, 1],
+    ])
+
+def get_rotate_z_matrix(angle):
+    angle = angle * pi / 180
+    return np.matrix([
+        [math.cos(angle), -math.sin(angle), 0, 0],
+        [math.sin(angle), math.cos(angle), 0, 0],
+        [0, 0, 1, 0],
+        [0, 0, 0, 1],
+    ])
+
+def matrix_mul(m, pt):
+    ptx = list(pt) + [1]
+    return list((m * np.matrix(ptx).T).T.flat)[:-1]
+
+def point_rotate_x(pt, angle):
+    ptx = list(pt) + [1]
+    return list((get_rotate_x_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
+
+def point_rotate_y(pt, angle):
+    ptx = list(pt) + [1]
+    return list((get_rotate_y_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
+
+def point_rotate_z(pt, angle):
+    ptx = list(pt) + [1]
+    return list((get_rotate_z_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]
+
+def path_rotate_x(path, angle):
+    return [point_rotate_x(p, angle) for p in path]
+
+def path_rotate_y(path, angle):
+    return [point_rotate_y(p, angle) for p in path]
+
+def path_rotate_z(path, angle):
+    return [point_rotate_z(p, angle) for p in path]
+
+if __name__ == '__main__':
+    pt = [0, 1, 0]
+    print(point_rotate_z(pt, 270))