Improve 2 dof simulation code

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)

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

ros2_ws/src/simple_robot_2dof/simple_robot_2dof/inverse_kinematics_2dof_node.py

-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