From 1998214f0557e23d6202c8954edb37a401cf502a Mon Sep 17 00:00:00 2001
From: Ventsi <uquzq@student.kit.edu>
Date: Mon, 28 Oct 2024 14:30:57 +0100
Subject: [PATCH] Clear inverse kinematics node code
---
.../iiwa_inverse_kinematics/inverse_kinematics_node.py | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
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 964b1c3..833a17c 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()
--
GitLab