trying to stomp

hexapod_python/ik_test.py

@@ -6,6 +6,7 @@ from hexapod_python.ik import ik
 from hexapod_python.ik_poses import interpolate_pose, standing_pose, stand_up_pose, shutdown_pose
 
 import time
+import copy
 
 
 class IKTestNode(IKPublisherNode):
@@ -13,7 +14,34 @@ class IKTestNode(IKPublisherNode):
             super().__init__('ik_test')
             ik_values = [ik(pose) for pose in interpolate_pose(shutdown_pose, stand_up_pose, 50)]
             self.publish_ik_values(ik_values)
-            time.sleep(5)
+            time.sleep(1)
+
+            current_pose = copy.deepcopy(stand_up_pose)
+
+            ik_values = []
+            for leg in self.leg_groups_:
+                intermediate_pose_1 = copy.deepcopy(current_pose)
+                intermediate_pose_1['legs'][leg].z += 20
+
+                intermediate_pose_2 = copy.deepcopy(intermediate_pose_1)
+                intermediate_pose_2['legs'][leg] = copy.deepcopy(standing_pose['legs'][leg])
+                intermediate_pose_2['legs'][leg].z += 20
+
+                intermediate_pose_3 = copy.deepcopy(intermediate_pose_2)
+                intermediate_pose_3['legs'][leg] = copy.deepcopy(standing_pose['legs'][leg])
+
+                ik_values.append(
+                    [ik(pose) for pose in interpolate_pose(current_pose, intermediate_pose_1, 50)] +
+                    [ik(pose) for pose in interpolate_pose(intermediate_pose_1, intermediate_pose_2, 50)] +
+                    [ik(pose) for pose in interpolate_pose(intermediate_pose_2, intermediate_pose_3, 50)]
+                )
+
+            self.publish_ik_values(ik_values)
+            time.sleep(20)
+
+            self.publish_ik_values(ik_values.reverse())
+            time.sleep(20)
+
             ik_values = [ik(pose) for pose in interpolate_pose(stand_up_pose, shutdown_pose, 50)]
             self.publish_ik_values(ik_values)
             raise SystemExit