diff --git a/ros2_ws/src/iiwa/iiwa_inverse_kinematics/iiwa_inverse_kinematics/inverse_kinematics_node.py b/ros2_ws/src/iiwa/iiwa_inverse_kinematics/iiwa_inverse_kinematics/inverse_kinematics_node.py index 964b1c32e82b930fecf27157c0f738d0a7eb11d9..833a17c2d368f37944bf85d38e4f34b4fd327775 100644 --- a/ros2_ws/src/iiwa/iiwa_inverse_kinematics/iiwa_inverse_kinematics/inverse_kinematics_node.py +++ b/ros2_ws/src/iiwa/iiwa_inverse_kinematics/iiwa_inverse_kinematics/inverse_kinematics_node.py @@ -1,5 +1,4 @@ import os -import re import rclpy from rclpy.node import Node @@ -21,6 +20,7 @@ class InverseKinematicsNode(Node): 10) self.publisher = self.create_publisher(JointTrajectory, '/iiwa_arm_controller/joint_trajectory', 10) + # load KUKA URDF urdf_file_path = os.path.join( get_package_share_directory('iiwa_description'), 'urdf', 'iiwa14.urdf') self.get_logger().info(f'Loading URDF from: {urdf_file_path}') @@ -35,7 +35,7 @@ class InverseKinematicsNode(Node): self.joint_names = [f'joint_a{i+1}' for i in range(len(self.Slist[0]))] self.joint_angles = np.zeros(len(self.Slist[0])) - + # forward kinematics T = FKinBody(self.M, self.Blist, self.joint_angles) rotation_matrix = T[:3, :3] quat = Rotation.from_matrix(rotation_matrix).as_quat()