diff --git a/hexapod_python/ik_poses.py b/hexapod_python/ik_poses.py index 293bef0..c10f65d 100644 --- a/hexapod_python/ik_poses.py +++ b/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) diff --git a/hexapod_python/step_test.py b/hexapod_python/step_test.py new file mode 100644 index 0000000..c7f243e --- /dev/null +++ b/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() + \ No newline at end of file diff --git a/setup.py b/setup.py index 64620c3..60b224b 100644 --- a/setup.py +++ b/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' ], }, )