step_test added

hexapod_python/ik_poses.py

@@ -3,31 +3,31 @@ from hexapod_python.ik import leg_offsets
 
 standing_pose = {
     'body': Pose(
-        position = Point(x = 0.0, y = 0.0, z = 105.0 ),
+        position = Point(x = 0.0, y = 0.0, z = 80.0 ),
         orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
     ),
     'legs': {
-        'e1': Point(x = leg_offsets['e1'].x + -100.0, y = leg_offsets['e1'].y, z = 0.0),
-        'e2': Point(x = leg_offsets['e2'].x + -100.0, y = leg_offsets['e2'].y, z = 0.0),
-        'e3': Point(x = leg_offsets['e3'].x + -100.0, y = leg_offsets['e3'].y, z = 0.0),
-        'e4': Point(x = leg_offsets['e4'].x +  100.0, y = leg_offsets['e4'].y, z = 0.0),
-        'e5': Point(x = leg_offsets['e5'].x +  100.0, y = leg_offsets['e5'].y, z = 0.0),
-        'e6': Point(x = leg_offsets['e6'].x +  100.0, y = leg_offsets['e6'].y, z = 0.0)
+        'e1': Point(x = leg_offsets['e1'].x +  -50.0, y = leg_offsets['e1'].y + 86.0, z = 0.0),
+        'e2': Point(x = leg_offsets['e2'].x + -100.0, y = leg_offsets['e2'].y +  0.0, z = 0.0),
+        'e3': Point(x = leg_offsets['e3'].x +  -50.0, y = leg_offsets['e3'].y - 86.0, z = 0.0),
+        'e4': Point(x = leg_offsets['e4'].x +   50.0, y = leg_offsets['e4'].y - 86.0, z = 0.0),
+        'e5': Point(x = leg_offsets['e5'].x +  100.0, y = leg_offsets['e5'].y +  0.0, z = 0.0),
+        'e6': Point(x = leg_offsets['e6'].x +   50.0, y = leg_offsets['e6'].y + 86.0, z = 0.0)
     }
 }
 
 stand_up_pose = {
     'body': Pose(
-        position = Point(x = 0.0, y = 0.0, z = 105.0 ),
+        position = Point(x = 0.0, y = 0.0, z = 80.0 ),
         orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
     ),
     'legs': {
-        'e1': Point(x = leg_offsets['e1'].x + -140.0, y = leg_offsets['e1'].y, z = 0.0),
-        'e2': Point(x = leg_offsets['e2'].x + -140.0, y = leg_offsets['e2'].y, z = 0.0),
-        'e3': Point(x = leg_offsets['e3'].x + -140.0, y = leg_offsets['e3'].y, z = 0.0),
-        'e4': Point(x = leg_offsets['e4'].x +  140.0, y = leg_offsets['e4'].y, z = 0.0),
-        'e5': Point(x = leg_offsets['e5'].x +  140.0, y = leg_offsets['e5'].y, z = 0.0),
-        'e6': Point(x = leg_offsets['e6'].x +  140.0, y = leg_offsets['e6'].y, z = 0.0)
+        'e1': Point(x = leg_offsets['e1'].x +  -70.0, y = leg_offsets['e1'].y + 121.0, z = 0.0),
+        'e2': Point(x = leg_offsets['e2'].x + -140.0, y = leg_offsets['e2'].y +   0.0, z = 0.0),
+        'e3': Point(x = leg_offsets['e3'].x +  -70.0, y = leg_offsets['e3'].y - 121.0, z = 0.0),
+        'e4': Point(x = leg_offsets['e4'].x +   70.0, y = leg_offsets['e4'].y - 121.0, z = 0.0),
+        'e5': Point(x = leg_offsets['e5'].x +  140.0, y = leg_offsets['e5'].y +   0.0, z = 0.0),
+        'e6': Point(x = leg_offsets['e6'].x +   70.0, y = leg_offsets['e6'].y + 121.0, z = 0.0)
     }
 }
 
@@ -39,12 +39,12 @@ shutdown_pose = {
         orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
     ),
     'legs': {
-        'e1': Point(x = leg_offsets['e1'].x + -140.0, y = leg_offsets['e1'].y, z = 0.0),
-        'e2': Point(x = leg_offsets['e2'].x + -140.0, y = leg_offsets['e2'].y, z = 0.0),
-        'e3': Point(x = leg_offsets['e3'].x + -140.0, y = leg_offsets['e3'].y, z = 0.0),
-        'e4': Point(x = leg_offsets['e4'].x +  140.0, y = leg_offsets['e4'].y, z = 0.0),
-        'e5': Point(x = leg_offsets['e5'].x +  140.0, y = leg_offsets['e5'].y, z = 0.0),
-        'e6': Point(x = leg_offsets['e6'].x +  140.0, y = leg_offsets['e6'].y, z = 0.0),
+        'e1': Point(x = leg_offsets['e1'].x +  -70.0, y = leg_offsets['e1'].y + 121.0, z = 0.0),
+        'e2': Point(x = leg_offsets['e2'].x + -140.0, y = leg_offsets['e2'].y +   0.0, z = 0.0),
+        'e3': Point(x = leg_offsets['e3'].x +  -70.0, y = leg_offsets['e3'].y - 121.0, z = 0.0),
+        'e4': Point(x = leg_offsets['e4'].x +   70.0, y = leg_offsets['e4'].y - 121.0, z = 0.0),
+        'e5': Point(x = leg_offsets['e5'].x +  140.0, y = leg_offsets['e5'].y +   0.0, z = 0.0),
+        'e6': Point(x = leg_offsets['e6'].x +   70.0, y = leg_offsets['e6'].y + 121.0, z = 0.0)
     }
 }
 
@@ -67,7 +67,7 @@ calibration_pose = {
 }
 
 
-def interpolate_pose(start, end, steps):
+def interpolate_pose(start, end, steps, fn = {}):
 
     body_step_x = (end['body'].position.x - start['body'].position.x) / (steps-1)
     body_step_y = (end['body'].position.y - start['body'].position.y) / (steps-1)
@@ -102,6 +102,11 @@ def interpolate_pose(start, end, steps):
                 y = start['legs'][leg].y + leg_step_y[leg] * i,
                 z = start['legs'][leg].z + leg_step_z[leg] * i
             )
+            if leg in fn:
+                (fn_x, fn_y, fn_z) = fn[leg](i)
+                pose['legs'][leg].x += fn_x
+                pose['legs'][leg].y += fn_y
+                pose['legs'][leg].z += fn_z
 
         poses.append(pose)
 

hexapod_python/step_test.py

@@ -0,0 +1,115 @@
+import rclpy
+from rclpy.node import Node
+
+from hexapod_python.ik_publisher_node import IKPublisherNode
+from hexapod_python.ik import ik
+from hexapod_python.ik_poses import interpolate_pose, standing_pose, stand_up_pose, shutdown_pose
+
+import math
+import time
+import copy
+import functools
+
+
+
+class StepTestNode(IKPublisherNode):
+    def __init__(self):
+            super().__init__('step_test')
+            ik_values = [ik(pose) for pose in interpolate_pose(shutdown_pose, stand_up_pose, 50)]
+            ik_values += self.step_interpolate(stand_up_pose, standing_pose, 2, 0, 20, 20)
+
+            # for l in range(1,4):
+
+            #     intermediate_pose = copy.deepcopy(current_pose)
+            #     intermediate_pose['legs'][f'e{l  }'] = target_pose['legs'][f'e{l  }']
+            #     intermediate_pose['legs'][f'e{l+3}'] = target_pose['legs'][f'e{l+3}']
+
+            #     interpolation_steps = 20
+            #     step_height = 20
+            #     ik_values += [ik(pose) for pose in interpolate_pose(current_pose, intermediate_pose, interpolation_steps, {
+            #                     f'e{l  }': lambda i : (0, 0, step_height * math.sin(i/(interpolation_steps-1) * math.pi)),
+            #                     f'e{l+3}': lambda i : (0, 0, step_height * math.sin(i/(interpolation_steps-1) * math.pi)),
+            #                 }
+            #     )]
+            #     current_pose = intermediate_pose
+
+            # for n in range(0, 10):
+                 
+
+
+            current_pose = standing_pose
+            for n in range(0,5):
+                step_x = 0
+                step_y = 50
+
+                step_height = 20
+                interpolation_steps = 25
+
+                fn = lambda i : (0, 0, step_height * math.sin(i/(interpolation_steps-1) * math.pi))
+
+                lift_leg_group = ['e1', 'e3', 'e5']                
+
+
+                step_pose = copy.deepcopy(current_pose)
+                step_fn = {}
+                for leg in self.leg_groups_:
+                    if leg in lift_leg_group:
+                        step_pose['legs'][leg].x += step_x / 2
+                        step_pose['legs'][leg].y += step_y / 2
+                        step_fn[leg] = fn
+                    else:
+                        step_pose['legs'][leg].x -= step_x / 2
+                        step_pose['legs'][leg].y -= step_y / 2
+
+                ik_values += [ik(pose) for pose in interpolate_pose(current_pose, step_pose, interpolation_steps, step_fn)]
+                step_fn = {}
+                for leg in self.leg_groups_:
+                    if not leg in lift_leg_group:
+                        step_fn[leg] = fn
+
+                ik_values += [ik(pose) for pose in interpolate_pose(step_pose, current_pose, interpolation_steps, step_fn)]
+
+
+            ik_values += self.step_interpolate(standing_pose, stand_up_pose, 2, 0, 20, 20)
+            ik_values += [ik(pose) for pose in interpolate_pose(stand_up_pose, shutdown_pose, 50)]
+
+            self.publish_ik_values(ik_values)
+            raise SystemExit
+
+
+    def step_interpolate(self, start_pose, target_pose, legs_at_once, leg_index_offset, interpolation_steps, step_height):
+        ik_values = []
+        current_pose = start_pose
+
+        n_legs = len(self.leg_groups_)
+        n_cycles = int(n_legs / legs_at_once)
+
+        for cycle in range(0, n_cycles): 
+            intermediate_pose = copy.deepcopy(current_pose)
+            intermediate_pose['body'].position.x += (target_pose['body'].position.x - start_pose['body'].position.x) 
+            intermediate_pose['body'].position.y += (target_pose['body'].position.y - start_pose['body'].position.y) 
+            intermediate_pose['body'].position.z += (target_pose['body'].position.z - start_pose['body'].position.z) 
+            step_fn = {}
+            for l in range(0, legs_at_once): # 0 .. 1
+                leg_num = (cycle + 1 + n_cycles * l + leg_index_offset)
+
+                while(leg_num > n_legs):
+                     leg_num -= n_legs
+
+                leg = f'e{leg_num}'
+                intermediate_pose['legs'][leg] = target_pose['legs'][leg]
+                step_fn[leg] = lambda i : (0, 0, step_height * math.sin(i/(interpolation_steps-1) * math.pi))
+
+            ik_values += [ik(pose) for pose in interpolate_pose(current_pose, intermediate_pose, interpolation_steps, step_fn)]
+            current_pose = intermediate_pose
+
+        return ik_values
+
+def main(args = None):
+    rclpy.init(args=args)
+
+    ik_publisher = StepTestNode()
+    rclpy.spin_once(ik_publisher)
+    ik_publisher.destroy_node()
+    rclpy.shutdown()
+    

setup.py

@@ -22,7 +22,8 @@ setup(
         'console_scripts': [
             'jtc_test = hexapod_python.jtc_test:main',
             'ik_test = hexapod_python.ik_test:main',
-            'ik_calibration = hexapod_python.ik_calibration:main'
+            'ik_calibration = hexapod_python.ik_calibration:main',
+            'step_test = hexapod_python.step_test:main'
         ],
     },
 )