More testing

hexapod_python/ik_test.py

@@ -30,17 +30,39 @@ class IKTestNode(IKPublisherNode):
                 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)]
+                ik_values += (
+                    [ik(pose) for pose in interpolate_pose(current_pose, intermediate_pose_1, 10)]+
+                    [ik(pose) for pose in interpolate_pose(intermediate_pose_1, intermediate_pose_2, 20)] +
+                    [ik(pose) for pose in interpolate_pose(intermediate_pose_2, intermediate_pose_3, 10)]
                 )
+                current_pose = copy.deepcopy(intermediate_pose_3)
 
             self.publish_ik_values(ik_values)
-            time.sleep(20)
+            time.sleep(8)
+
+            current_pose = copy.deepcopy(standing_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(stand_up_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(stand_up_pose['legs'][leg])
+
+                ik_values += (
+                    [ik(pose) for pose in interpolate_pose(current_pose, intermediate_pose_1, 10)]+
+                    [ik(pose) for pose in interpolate_pose(intermediate_pose_1, intermediate_pose_2, 20)] +
+                    [ik(pose) for pose in interpolate_pose(intermediate_pose_2, intermediate_pose_3, 10)]
+                )
+                current_pose = copy.deepcopy(intermediate_pose_3)
+
+            self.publish_ik_values(ik_values)
+            time.sleep(6)
 
-            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)