update

software/raspberry pi/hexapod.py

@@ -40,7 +40,7 @@ import json
 from path_generator import gen_walk_path
 from path_generator import gen_fastwalk_path
 from path_generator import gen_turn_path
-from path_generator import gen_shift_path
+# from path_generator import gen_shift_path
 from path_generator import gen_climb_path
 from path_generator import gen_rotatex_path, gen_rotatey_path, gen_rotatez_path
 from path_generator import gen_twist_path
@@ -55,15 +55,20 @@ class Hexapod(Thread):
     CMD_STANDBY = 'standby'
     CMD_LAYDOWN = 'laydown'
 
-    CMD_FORWARD = 'forward'
+    CMD_WALK_0 = 'walk0'
-    CMD_BACKWARD = 'backward'
+    CMD_WALK_180 = 'walk180'
+
+    CMD_WALK_R45 = 'walkr45'
+    CMD_WALK_R90 = 'walkr90'
+    CMD_WALK_R135 = 'walkr135'
+
+    CMD_WALK_L45 = 'walkl45'
+    CMD_WALK_L90 = 'walkl90'
+    CMD_WALK_L135 = 'walkl135'
 
     CMD_FASTFORWARD = 'fastforward'
     CMD_FASTBACKWARD = 'fastbackward'
 
-    CMD_SHIFTLEFT = 'shiftleft'
-    CMD_SHIFTRIGHT = 'shiftright'
-
     CMD_TURNLEFT = 'turnleft'
     CMD_TURNRIGHT = 'turnright'
 
@@ -153,10 +158,22 @@ class Hexapod(Thread):
         self.cmd_dict = {
             self.CMD_STANDBY: self.standby_posture,
             self.CMD_LAYDOWN: self.gen_posture(0, 15),
-            self.CMD_FORWARD: gen_walk_path(
+            self.CMD_WALK_0: gen_walk_path(
-                self.standby_posture['coord']),
+                self.standby_posture['coord'], direction=0),
-            self.CMD_BACKWARD: gen_walk_path(
+            self.CMD_WALK_180: gen_walk_path(
-                self.standby_posture['coord'], reverse=True),
+                self.standby_posture['coord'], direction=180),
+            self.CMD_WALK_R45: gen_walk_path(
+                self.standby_posture['coord'], direction=315),
+            self.CMD_WALK_R90: gen_walk_path(
+                self.standby_posture['coord'], direction=270),
+            self.CMD_WALK_R135: gen_walk_path(
+                self.standby_posture['coord'], direction=225),
+            self.CMD_WALK_L45: gen_walk_path(
+                self.standby_posture['coord'], direction=45),
+            self.CMD_WALK_L90: gen_walk_path(
+                self.standby_posture['coord'], direction=90),
+            self.CMD_WALK_L135: gen_walk_path(
+                self.standby_posture['coord'], direction=135),
             self.CMD_FASTFORWARD: gen_fastwalk_path(
                 self.standby_posture['coord']),
             self.CMD_FASTBACKWARD: gen_fastwalk_path(
@@ -165,10 +182,6 @@ class Hexapod(Thread):
                 self.standby_posture['coord'], direction='left'),
             self.CMD_TURNRIGHT: gen_turn_path(
                 self.standby_posture['coord'], direction='right'),
-            self.CMD_SHIFTLEFT: gen_shift_path(
-                self.standby_posture['coord'], direction='left'),
-            self.CMD_SHIFTRIGHT: gen_shift_path(
-                self.standby_posture['coord'], direction='right'),
             self.CMD_CLIMBFORWARD: gen_climb_path(
                 self.standby_posture['coord'], reverse=False),
             self.CMD_CLIMBBACKWARD: gen_climb_path(

software/raspberry pi/path_generator.py

@@ -30,21 +30,46 @@ from lib import get_rotate_x_matrix, get_rotate_y_matrix, get_rotate_z_matrix
 import numpy as np
 
 
+# def gen_walk_path(standby_coordinate,
+#                   g_steps=20,
+#                   g_radius=25,
+#                   reverse=False):
+#     assert (g_steps % 4) == 0
+
+#     halfsteps = int(g_steps/2)
+
+#     path = np.zeros((g_steps, 6, 3))
+
+#     semi_circle = semicircle_generator(g_radius, g_steps, reverse=reverse)
+
+#     path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
+#     path[:, [1, 3, 5], :] = np.tile(
+#         np.roll(semi_circle[:, np.newaxis, :], halfsteps, axis=0), (1, 3, 1))
+
+#     return {'coord': path+np.tile(standby_coordinate, (g_steps, 1, 1)),
+#             'type': 'motion'}
+
+
 def gen_walk_path(standby_coordinate,
                   g_steps=20,
                   g_radius=25,
-                  reverse=False):
+                  direction=0):
     assert (g_steps % 4) == 0
-
     halfsteps = int(g_steps/2)
 
-    path = np.zeros((g_steps, 6, 3))
+    # if direction == 'left':
+    #     shift_angle = 90
+    # elif direction == 'right':
+    #     shift_angle = 270
 
-    semi_circle = semicircle_generator(g_radius, g_steps, reverse=reverse)
+    semi_circle = semicircle_generator(g_radius, g_steps)
+    # shift 90 degree to make the path "left" shift
+    semi_circle = np.array(path_rotate_z(semi_circle, direction))
+    mir_path = np.roll(semi_circle, halfsteps, axis=0)
 
+    path = np.zeros((g_steps, 6, 3))
     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(mir_path[:, np.newaxis, :], (1, 3, 1))
-        np.roll(semi_circle[:, np.newaxis, :], halfsteps, axis=0), (1, 3, 1))
 
     return {'coord': path+np.tile(standby_coordinate, (g_steps, 1, 1)),
             'type': 'motion'}
@@ -107,31 +132,6 @@ def gen_turn_path(standby_coordinate,
             'type': 'motion'}
 
 
-def gen_shift_path(standby_coordinate,
-                   g_steps=20,
-                   g_radius=25,
-                   direction='left'):
-    assert (g_steps % 4) == 0
-    halfsteps = int(g_steps/2)
-
-    if direction == 'left':
-        shift_angle = 90
-    elif direction == 'right':
-        shift_angle = 270
-
-    semi_circle = semicircle_generator(g_radius, g_steps)
-    # shift 90 degree to make the path "left" shift
-    semi_circle = np.array(path_rotate_z(semi_circle, shift_angle))
-    mir_path = np.roll(semi_circle, halfsteps, axis=0)
-
-    path = np.zeros((g_steps, 6, 3))
-    path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
-    path[:, [1, 3, 5], :] = np.tile(mir_path[:, np.newaxis, :], (1, 3, 1))
-
-    return {'coord': path+np.tile(standby_coordinate, (g_steps, 1, 1)),
-            'type': 'motion'}
-
-
 def gen_climb_path(standby_coordinate,
                    g_steps=20,
                    y_radius=20,