From 63edf3282859783a437184460b01c3cb66bc6afe Mon Sep 17 00:00:00 2001
From: Ventsi <uquzq@student.kit.edu>
Date: Tue, 16 Jul 2024 17:42:30 +0200
Subject: [PATCH] Improve 2 dof simulation code
---
.../forward_kinematics_node_2dof_1.py | 7 +--
.../forward_kinematics_node_2dof_2.py | 19 ++-----
.../inverse_kinematics_2dof_node.py | 53 -------------------
3 files changed, 5 insertions(+), 74 deletions(-)
delete mode 100644 ros2_ws/src/simple_robot_2dof/simple_robot_2dof/inverse_kinematics_2dof_node.py
diff --git a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
index 802e56a..1988ebc 100644
--- a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
+++ b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
@@ -29,7 +29,6 @@ class Forward_Kinematics_Node_2Dof(Node):
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}')
self.robot_model = loadURDF(urdf_file_path)
self.M = self.robot_model["M"]
self.Slist = self.robot_model["Slist"]
@@ -45,7 +44,7 @@ class Forward_Kinematics_Node_2Dof(Node):
self.height_publisher_ = self.create_publisher(Float64, '/simple_robot/height_2dof/plotjuggler', 10)
# create a publisher for the pose
self.pose_publisher_simple_robot_ = self.create_publisher(PoseStamped, '/simple_robot/end_effector_pose', 10)
- self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, 'target_pose', 10)
+ self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, '/kuka_iiwa/target_pose', 10)
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -72,9 +71,6 @@ class Forward_Kinematics_Node_2Dof(Node):
self.publish_height(h_fraction)
- # Define the vector
- # start_vector = np.array([1.0, 0.0, 0.0, 1.0])
-
# Define the matrix
intermediate_rotation = np.array([
[cos(theta_fraction), -sin(theta_fraction), 0, 0],
@@ -98,6 +94,7 @@ class Forward_Kinematics_Node_2Dof(Node):
end_position = end_forward_kinematics[:4, 3]
end_orientation = Rotation.from_matrix(end_forward_kinematics[:3, :3]).as_quat()
+ self.publish_pose_simple_robot(end_position, end_orientation)
self.publish_pose_target_pose(end_position, end_orientation)
self.construct_path(end_position)
diff --git a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py
index 245a972..e0fe2a9 100644
--- a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py
+++ b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py
@@ -29,7 +29,6 @@ class Forward_Kinematics_Node_2Dof(Node):
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}')
self.robot_model = loadURDF(urdf_file_path)
self.M = self.robot_model["M"]
self.Slist = self.robot_model["Slist"]
@@ -43,7 +42,7 @@ class Forward_Kinematics_Node_2Dof(Node):
self.index = 0
self.pose_publisher_simple_robot_ = self.create_publisher(PoseStamped, '/simple_robot/end_effector_pose', 10)
- self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, 'target_pose', 10)
+ self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, '/kuka_iiwa/target_pose', 10)
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -71,9 +70,6 @@ class Forward_Kinematics_Node_2Dof(Node):
# split theta
theta_fraction = self.phi * theta
- # Define the vector
- # start_vector = np.array([1.0, 0.0, 0.0, 1.0])
-
# define the matrix
intermediate_rotation = np.array([
[cos(theta_fraction), -sin(theta_fraction), 0, 0],
@@ -102,15 +98,11 @@ class Forward_Kinematics_Node_2Dof(Node):
end_position_1 = self.end_forward_kinematics_1[:4, 3]
end_orientation_1 = Rotation.from_matrix(self.end_forward_kinematics_1[:3, :3]).as_quat()
-
- # calculate the intermediate vectors
- # rotated_intermediate_vector = np.matmul(intermediate_rotation, start_vector)
-
- # quaternion_1 = transformations.quaternion_from_euler(0.0, 0.0, theta_fraction)
# calculate the end vector
self.end_vector = np.matmul(rotation, start_position)
+ self.publish_pose_simple_robot(end_position_1, end_orientation_1)
self.publish_pose_target_pose(end_position_1, end_orientation_1)
# construct the path of the rotation
@@ -135,15 +127,10 @@ class Forward_Kinematics_Node_2Dof(Node):
end_position_2 = end_forward_kinematics_2[:4, 3]
end_orientation_2 = Rotation.from_matrix(end_forward_kinematics_2[:3, :3]).as_quat()
- # calculate the intermediate vectors
- translated_vector = np.matmul(intermediate_translation, self.end_vector)
-
- # quaternion_2 = transformations.quaternion_from_euler(0.0, 0.0, - pi /2)
-
self.publish_pose_target_pose(end_position_2, end_orientation_2)
# construct the path of the translation
- self.construct_path(translated_vector)
+ self.construct_path(end_position_2)
def construct_path(self, vector):
# RViz
diff --git a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/inverse_kinematics_2dof_node.py b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/inverse_kinematics_2dof_node.py
deleted file mode 100644
index ae1104c..0000000
--- a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/inverse_kinematics_2dof_node.py
+++ /dev/null
@@ -1,53 +0,0 @@
-import rclpy
-from rclpy.node import Node
-from geometry_msgs.msg import Twist
-from math import sin, cos, pi
-import numpy as np
-
-class Transformation2DoFNode(Node):
-
- def __init__(self):
- super().__init__('transformation_2dof_node')
- self.get_logger().info('2DoF Transformation Node Initialized')
- self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)#Publishing data to the cmd_vel node
-
- def transform_2dof(self, theta, h):
- #Define 4x4 rotation matrix
- rotation_matrix = np.array([
- [cos(theta), -sin(theta), 0, 0],
- [sin(theta), cos(theta), 0, 0],
- [0, 0, 1, 0],
- [0, 0, 0, 1]
- ])
- #Define 4x4 translation matrix
- translation_matrix = np.array([
- [1, 0, 0, 0],
- [0, 1, 0, 0],
- [0, 0, 1, h],
- [0, 0, 0, 1]
- ])
- #Build the matrix product
- product_matrix = np.dot(translation_matrix, rotation_matrix)
-
- self.command_robot_rotation(product_matrix)
-
- def command_robot_rotation(self, transformation_matrix):#sth is wrong here
- #twist = Twist()
- #twist.angular.z = theta
- #self.publisher_publish(twist)
- self.get_logger().info('Applied 2DOF transformation: \n{}'.format(transformation_matrix))
-
-def main(args=None):
- rclpy.init(args=args)
- node = Transformation2DoFNode()
-
- node.transform_2dof(pi / 2, 50)#Set value of theta and h here
-
- rclpy.spin(node)
- node.destroy_node()
- rclpy.shutdown
-
-if __name__ == '__main__':
- main()
-
-
\ No newline at end of file
--
GitLab