Skip to content
Snippets Groups Projects
Commit 1998214f authored by Ventsislav Dokusanski's avatar Ventsislav Dokusanski
Browse files

Clear inverse kinematics node code

parent 56320a17
Branches main
No related tags found
No related merge requests found
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()
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment