Create 3 dof simulation using the inverse kinematics package and urdf file

ros2_ws/src/simple_robot_3dof/launch/forward_kinematics_3dof_sequential_launch.py

@@ -35,5 +35,12 @@ def generate_launch_description():
             name='rviz2',
             output='screen',
             arguments=['-d', rviz_config_file]
+        ),
+        Node(
+            package= 'iiwa_inverse_kinematics',
+            executable='inverse_kinematics_node',
+            name='inverse_kinematics',
+            output='screen',
+            parameters = []
         )
     ])
\ No newline at end of file

ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py

 import rclpy
+import os
+from modern_robotics import FKinBody
+from ament_index_python.packages import get_package_share_directory
+from mr_urdf_loader import loadURDF
 from rclpy.node import Node
 from math import sin, cos, pi
 import numpy as np
 from std_msgs.msg import Float64
 from nav_msgs.msg import Path
 from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import Pose
 from visualization_msgs.msg import Marker
 import transformations
+from scipy.spatial.transform import Rotation
+import time
 
 
 class Forward_Kinematics_Node_3Dof(Node):
@@ -19,6 +26,16 @@ class Forward_Kinematics_Node_3Dof(Node):
             '/dof3/reference_angles',
             self.listener_callback,
             10)
+        
+
+        urdf_file_path = os.path.join(
+            get_package_share_directory('iiwa_description'), 'urdf', 'iiwa14.urdf')
+        self.robot_model = loadURDF(urdf_file_path)
+        self.M = self.robot_model["M"]
+        self.Slist = self.robot_model["Slist"]
+        self.Mlist = self.robot_model["Mlist"]
+        self.Glist = self.robot_model["Glist"]
+        self.Blist = self.robot_model["Blist"]
         # Create two publishers for the RViz simulation - one for the whole path and one for the marker
         self.path_publisher_ = self.create_publisher(Path, '/simple_robot/visualization_path', 10)
         self.point_publisher_ = self.create_publisher(Marker, '/simple_robot/visualization_marker', 10)
@@ -28,7 +45,7 @@ class Forward_Kinematics_Node_3Dof(Node):
         self.length_publisher_ = self.create_publisher(Float64, '/simple_robot/length_3dof/plotjuggler', 10)
 
         self.pose_publisher_simple_robot_ = self.create_publisher(PoseStamped, '/simple_robot/end_effector_pose', 10)
-        self.pose_publisher_kuka_robot_ = self.create_publisher(PoseStamped, '/kuka_robot/end_effector_pose', 10)
+        self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, '/target_pose', 10)
         #RViz
         self.path = Path()
         self.path.header.frame_id = "map"
@@ -65,35 +82,32 @@ class Forward_Kinematics_Node_3Dof(Node):
             [0, 0, 1, 0],
             [0, 0, 0, 1]
         ])
-        
-        # calculate the intermediate vectors
-        rotated_intermediate_vector = np.matmul(intermediate_rotation, self.end_vector)
 
-        quaternion_1 = transformations.quaternion_from_euler(0.0, 0.0, theta_fraction)
+        end_forward_kinematics_2 = np.matmul(intermediate_rotation, self.end_forward_kinematics_1)
 
-        self.publish_pose(rotated_intermediate_vector, quaternion_1)
+        end_position_2 = end_forward_kinematics_2[:4, 3]
+        end_orientation_2 = Rotation.from_matrix(end_forward_kinematics_2[:3, :3]).as_quat()
+
+        self.publish_pose_target_pose(end_position_2, end_orientation_2)
 
         # construct the path of the rotation
-        self.construct_path(rotated_intermediate_vector)
+        self.construct_path(end_position_2)
 
     def translation(self):
         # definte the desired height
-        h = 0.6
+        h = 0.2
         # split h
         h_fraction = self.phi * h
 
         self.publish_height(h_fraction)
 
         # define the desired length 
-        d = 0.35
+        d = 0.22
         # split d
         d_fraction = self.phi * d
 
         self.publish_length(d_fraction)
 
-        # Define the vector
-        start_vector = np.array([1.0, 0.0, 0.0, 1.0])
-
         # define translation matrix
         intermediate_translation = np.array([
             [1, 0, 0, 0],
@@ -109,15 +123,28 @@ class Forward_Kinematics_Node_3Dof(Node):
             [0, 0, 0, 1]
         ])
 
+        self.joint_angles = np.zeros(len(self.Slist[0]))
+        self.joint_angles[1] = np.pi/2
+        self.joint_angles[3] = np.pi/2
+        self.joint_angles[5] = np.pi/2
+
+        start_forward_kinematics = FKinBody(self.M, self.Blist, self.joint_angles)
+
+        start_position = start_forward_kinematics[:4, 3]
+        start_orientation = Rotation.from_matrix(start_forward_kinematics[:3, :3]).as_quat()
+
+        self.end_forward_kinematics_1 = np.matmul(intermediate_translation, start_forward_kinematics)
+
+        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
-        translated_vector = np.matmul(intermediate_translation, start_vector)
+        translated_vector = np.matmul(intermediate_translation, start_position)
 
         # calculate the end vector
-        self.end_vector = np.matmul(translation_matrix, start_vector)
-
-        quaternion_2 = transformations.quaternion_from_euler(0.0, 0.0, - pi / 2)
+        self.end_vector = np.matmul(translation_matrix, start_position)
 
-        self.publish_pose(translated_vector, quaternion_2)
+        self.publish_pose_target_pose(translated_vector, end_orientation_1)
 
         # construct the path of the translation
         self.construct_path(translated_vector)
@@ -167,7 +194,6 @@ class Forward_Kinematics_Node_3Dof(Node):
         angle_msg = Float64()
         angle_msg.data = angle * 180/ pi
         self.angle_publisher_.publish(angle_msg)
-        # self.get_logger().info('Publishing: %f' %angle_radians)
 
     def publish_height(self, height):
         height_msg = Float64()
@@ -179,7 +205,7 @@ class Forward_Kinematics_Node_3Dof(Node):
         length_msg.data = length
         self.length_publisher_.publish(length_msg)
 
-    def publish_pose(self, vector, quaternion):
+    def publish_pose_simple_robot(self, vector, quaternion):
         pose_msg = PoseStamped()
         pose_msg.header.frame_id = "map"
         pose_msg.header.stamp = self.get_clock().now().to_msg()
@@ -191,7 +217,20 @@ class Forward_Kinematics_Node_3Dof(Node):
         pose_msg.pose.orientation.z = quaternion[2]
         pose_msg.pose.orientation.w = quaternion[3]
         self.pose_publisher_simple_robot_.publish(pose_msg)
+
+    def publish_pose_target_pose(self, vector, quaternion):    
+        # Create a Pose message for the target pose
+        pose_msg = Pose()
+        pose_msg.position.x = vector[0]
+        pose_msg.position.y = vector[1]
+        pose_msg.position.z = vector[2]
+        pose_msg.orientation.x = quaternion[0]
+        pose_msg.orientation.y = quaternion[1]
+        pose_msg.orientation.z = quaternion[2]
+        pose_msg.orientation.w = quaternion[3]
         self.pose_publisher_kuka_robot_.publish(pose_msg)
+        # if self.phi == 0.0:
+        #     time.sleep(1)
 
 
 def main(args = None):