diff --git a/hexapod_python/ik.py b/hexapod_python/ik.py new file mode 100644 index 0000000..2bee3c7 --- /dev/null +++ b/hexapod_python/ik.py @@ -0,0 +1,166 @@ +import math + +from geometry_msgs.msg import Point, Pose, Quaternion +from trajectory_msgs.msg import JointTrajectoryPoint + + +leg_offsets = { + 'e1': Point(x = -40.0, y = 0.0, z = 0.0), + 'e2': Point(x = -60.0, y = 0.0, z = 0.0), + 'e3': Point(x = -40.0, y = -80.0, z = 0.0), + 'e4': Point(x = 40.0, y = 80.0, z = 0.0), + 'e5': Point(x = 60.0, y = 0.0, z = 0.0), + 'e6': Point(x = 40.0, y = -80.0, z = 0.0) +} + +leg_sides = { + 'e1': 'l', + 'e2': 'l', + 'e3': 'l', + 'e4': 'r', + 'e5': 'r', + 'e6': 'r', +} + +standing_pose = { + 'body': Pose( + position = Point(x = 0.0, y = 0.0, z = 0.0 ), + orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 0.0) + ), + 'legs': { + 'e1': Point(x = leg_offsets['e1'].x + -100.0, y = leg_offsets['e1'].y + 0.0, z =leg_offsets['e1'].z + -130.0), + 'e2': Point(x = leg_offsets['e2'].x + -100.0, y = leg_offsets['e2'].y + 0.0, z =leg_offsets['e2'].z + -130.0), + 'e3': Point(x = leg_offsets['e3'].x + -100.0, y = leg_offsets['e3'].y + 0.0, z =leg_offsets['e3'].z + -130.0), + 'e4': Point(x = leg_offsets['e4'].x + 100.0, y = leg_offsets['e4'].y + 0.0, z =leg_offsets['e4'].z + -130.0), + 'e5': Point(x = leg_offsets['e5'].x + 100.0, y = leg_offsets['e5'].y + 0.0, z =leg_offsets['e5'].z + -130.0), + 'e6': Point(x = leg_offsets['e6'].x + 100.0, y = leg_offsets['e6'].y + 0.0, z =leg_offsets['e6'].z + -130.0), + } +} + +shutdown_pose = { + 'body': Pose( + position = Point(x = 0.0, y = 0.0, z = 0.0 ), + orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 0.0) + ), + 'legs': { + 'e1': Point(x = leg_offsets['e1'].x + -140.0, y = leg_offsets['e1'].y, z = 8.0), + 'e2': Point(x = leg_offsets['e2'].x + -140.0, y = leg_offsets['e2'].y, z = 8.0), + 'e3': Point(x = leg_offsets['e3'].x + -140.0, y = leg_offsets['e3'].y, z = 8.0), + 'e4': Point(x = leg_offsets['e4'].x + 140.0, y = leg_offsets['e4'].y, z = 8.0), + 'e5': Point(x = leg_offsets['e5'].x + 140.0, y = leg_offsets['e5'].y, z = 8.0), + 'e6': Point(x = leg_offsets['e6'].x + 140.0, y = leg_offsets['e6'].y, z = 8.0), + } +} + +def interpolate_pose(start, end, steps): + + 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) + body_step_z = end['body'].position.z - start['body'].position.z / (steps-1) + + leg_step_x = {} + leg_step_y = {} + leg_step_z = {} + + for leg in start['legs']: + leg_step_x[leg] = (end['legs'][leg].x - start['legs'][leg].x) / (steps-1) + leg_step_y[leg] = (end['legs'][leg].y - start['legs'][leg].y) / (steps-1) + leg_step_z[leg] = (end['legs'][leg].z - start['legs'][leg].z) / (steps-1) + + poses = [] + for i in range(0, steps): + pose = { + 'body': Pose( + position = Point( + x = start['body'].position.x + body_step_x * i, + y = start['body'].position.y + body_step_y * i, + z = start['body'].position.z + body_step_z * i + ), + orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 0.0) + ), + 'legs': {} + } + + for leg in start['legs']: + pose['legs'][leg] = Point( + x = start['legs'][leg].x + leg_step_x[leg] * i, + y = start['legs'][leg].y + leg_step_y[leg] * i, + z = start['legs'][leg].z + leg_step_z[leg] * i + ) + + poses.append(pose) + + return poses + + + + +def ik(body_pose): + # todo apply rotation and translation of body + + joint_values = {} + for l in body_pose['legs']: + joint_values[l] = leg_ik( + leg_sides[l], + Point( + x = body_pose['legs'][l].x - leg_offsets[l].x, + y = body_pose['legs'][l].y - leg_offsets[l].y, + z = body_pose['legs'][l].z - leg_offsets[l].z, + ) + ) + + return joint_values + + + + +def leg_ik(side, point): + + x = point.x + y = point.y + z = point.z + + c = 20 + f = 80 + t = 130 + + ff = f*f + tt = t*t + + if side == 'l': + x = -x + + coxa_angle = math.atan2(y,x) + + r = math.sqrt(x*x + y * y) - c + + rr = r*r + zz = z*z + dd = rr + zz + d = math.sqrt(dd) + + femur_angle = math.acos((ff + dd - tt) / (2 * f * d)) + math.acos(-z/d) - math.pi/2 + tibia_angle = math.acos((tt + ff - dd) / (2 * t * f)) - math.pi/2 + + if(side == 'l'): + femur_angle *= -1 + else: + tibia_angle *= -1 + + + return([coxa_angle, femur_angle, tibia_angle]) + +if __name__ == "__main__": + print(leg_ik('l', Point(x = -100.0, y = 0.0, z = -130.0))) + print(leg_ik('r', Point(x = 100.0, y = 0.0, z = -130.0))) + print(leg_ik('l', Point(x = -120.0, y = 0.0, z = -130.0))) + print(leg_ik('r', Point(x = 120.0, y = 0.0, z = -130.0))) + + for o in leg_offsets: + print(leg_offsets[o]) + + + print(ik(standing_pose)) + print(ik(shutdown_pose)) + + print(interpolate_pose(standing_pose, shutdown_pose, 10)) diff --git a/hexapod_python/ik_test.py b/hexapod_python/ik_test.py new file mode 100644 index 0000000..750f092 --- /dev/null +++ b/hexapod_python/ik_test.py @@ -0,0 +1,60 @@ +from geometry_msgs.msg import Point, Pose +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from builtin_interfaces.msg import Duration +import rclpy +from rclpy.node import Node + +from hexapod_python.ik import ik, interpolate_pose, standing_pose, shutdown_pose + +import time + + +class JTCPublisher(Node): + def __init__(self): + super().__init__('jtc_publisher') + + self.leg_groups_ = [ "e1", "e2", "e3", "e4", "e5", "e6"] + self.publisher_ = {} + + for leg in self.leg_groups_: + controller_topic = f"/{leg}_group_controller/joint_trajectory" + self.publisher_[leg] = self.create_publisher(JointTrajectory, controller_topic , 10) + + + # wait for the controller to subscribe + print("Waiting for subscribers") + for tag in self.leg_groups_: + while self.count_subscribers(f"/{leg}_group_controller/joint_trajectory") == 0: + time.sleep(0.1) + + ik_values = [ ik(pose) for pose in interpolate_pose(shutdown_pose, standing_pose, 150) + interpolate_pose(standing_pose, shutdown_pose, 150)] + print(ik_values) + for leg in self.leg_groups_: + t = 0 + joint_trajectory_points = [] + for i in ik_values: + joint_trajectory_points.append( + JointTrajectoryPoint( + positions = i[leg], + time_from_start = Duration(nanosec = t%50 * 20000000 , sec = int(t/50)) + ) + ) + t += 1 + + + print(joint_trajectory_points) + group_num = leg[1:] + trajectory = JointTrajectory( + joint_names = [ f"c{group_num}_joint", f"f{group_num}_joint", f"t{group_num}_joint" ], + points = joint_trajectory_points + ) + self.publisher_[leg].publish(trajectory) + +def main(args = None): + rclpy.init(args=args) + + jtc_publisher = JTCPublisher() + rclpy.spin(jtc_publisher) + jtc_publisher.destroy_node() + rclpy.shutdown() + \ No newline at end of file diff --git a/package.xml b/package.xml index 3486b8b..7521855 100644 --- a/package.xml +++ b/package.xml @@ -10,6 +10,7 @@ rclpy std_msgs trajectory_msgs + geometry_msgs ament_copyright ament_flake8 diff --git a/setup.py b/setup.py index 9b3a50b..160e53e 100644 --- a/setup.py +++ b/setup.py @@ -20,7 +20,8 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'jtc_test = hexapod_python.jtc_test:main' + 'jtc_test = hexapod_python.jtc_test:main', + 'ik_test = hexapod_python.ik_test:main' ], }, )