Create simulation using the inverse kinematics package and urdf file

ros2_ws/src/python_parameters/python_parameters/velocity_parameter.py

 import rclpy
 from rclpy.node import Node
 from rclpy.parameter import Parameter
+from std_msgs.msg import Float64
 
 class VelocityParam(Node):
     def __init__(self):
@@ -8,13 +9,19 @@ class VelocityParam(Node):
         # Declare a parameter named 'velocity' with a default value of 10
         self.declare_parameter('velocity', 10.0)
 
+        self.velocity_publisher_ = self.create_publisher(Float64, '/simple_robot/velocity', 10)
+
         # Create a timer that triggers the timer_callback every second
-        self.timer = self.create_timer(1, self.timer_callback)
+        # self.timer = self.create_timer(1, self.timer_callback)
+        self.timer_callback()
 
     def timer_callback(self):
         # Get the current value of the 'velocity' parameter
         velocity = self.get_parameter('velocity').value
-        self.get_logger().info(f'Current velocity: {velocity}')
+        msg = Float64()
+        msg.data = velocity
+        self.velocity_publisher_.publish(msg)
+        self.get_logger().info(f'Current velocity: {msg.data}')
 
 def main():
     rclpy.init()

ros2_ws/src/simple_robot_1dof/launch/forward_kinematics_1dof_launch.py

@@ -35,5 +35,11 @@ def generate_launch_description():
             name='rviz2',
             output='screen',
             arguments=['-d', rviz_config_file]
-        )
+        ),
+        # Node(
+        #     package = 'python_parameters', 
+        #     executable='velocity_param_node',
+        #     name='velocity_param_node',
+        #     output='screen'
+        # )
     ])
\ No newline at end of file

ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.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
 
 class Forward_Kinematics_Node_1Dof(Node):
 
@@ -18,6 +24,17 @@ class Forward_Kinematics_Node_1Dof(Node):
             '/dof1/reference_angles',
             self.listener_callback,
             10)
+        
+        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"]
+        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)
@@ -25,7 +42,7 @@ class Forward_Kinematics_Node_1Dof(Node):
         self.angle_publisher_ = self.create_publisher(Float64, '/simple_robot/angles_1dof/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(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"
@@ -39,15 +56,12 @@ class Forward_Kinematics_Node_1Dof(Node):
         
     def transformation(self):
         # Define the desired rotation here
-        theta = pi / 2
+        theta = pi / 1.8
         # Split theta
         theta_fraction = self.phi * theta
 
         self.publish_angle(theta_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],
@@ -55,17 +69,30 @@ class Forward_Kinematics_Node_1Dof(Node):
             [0, 0, 1, 0],
             [0, 0, 0, 1]
         ])
-        
-        # Calculate the intermediate vectors
-        intermediate_vector = np.matmul(intermediate_rotation, start_vector)
 
-        quaternion = transformations.quaternion_from_euler(0.0, 0.0, theta_fraction)
+        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()
+
+        end_forward_kinematics = np.matmul(intermediate_rotation, start_forward_kinematics)
+
+        end_position = end_forward_kinematics[:4, 3]
+        end_orientation = Rotation.from_matrix(end_forward_kinematics[:3, :3]).as_quat()
+            
+        # Calculate the intermediate vectors
+        intermediate_vector = np.matmul(intermediate_rotation, start_position)
         
-        self.publish_pose(intermediate_vector, quaternion)
+        self.publish_pose_target_pose(end_position, end_orientation)
         
-        self.construct_path(intermediate_vector, quaternion)
+        self.construct_path(intermediate_vector)
 
-    def construct_path(self, vector, quaternion):
+    def construct_path(self, vector):
         # RViz
         pose = PoseStamped()
         pose.header.frame_id = "map"
@@ -73,7 +100,7 @@ class Forward_Kinematics_Node_1Dof(Node):
         pose.pose.position.x = vector[0]
         pose.pose.position.y = vector[1]
         pose.pose.position.z = vector[2]
-        pose.pose.orientation.w = quaternion[3]
+        pose.pose.orientation.w = 1.0
 
         # if the path is completed clear it
         if self.phi == 0:
@@ -113,7 +140,7 @@ class Forward_Kinematics_Node_1Dof(Node):
         self.angle_publisher_.publish(angle_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()
@@ -125,9 +152,20 @@ class Forward_Kinematics_Node_1Dof(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)
 
-         
+
 def main(args = None):
     rclpy.init(args = args)
     node = Forward_Kinematics_Node_1Dof()

ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py

@@ -12,7 +12,7 @@ class Reference_Node_1DoF(Node):
         super().__init__('reference_node_1dof')
         #Create a publisher sending the vector as a message
         self.publisher_ = self.create_publisher(Float64, '/dof1/reference_angles', 10)
-        timer_period = 0.05  # seconds
+        timer_period = 0.1  # seconds
         self.timer = self.create_timer(timer_period, self.ref_point_publisher)
         self.phi_start = 0.0
         self.phi_end = 1.0

ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof_velocity.py

+import rclpy
+from rclpy.node import Node
+import numpy as np
+from std_msgs.msg import Float64
+from geometry_msgs.msg import Point, PoseStamped
+import time
+
+
+class Reference_Node_1DoF_Velocity(Node):
+
+    def __init__(self):
+        super().__init__('reference_node_1dof_velocity')
+        self.subscription = self.create_subscription(
+            Float64,
+            '/simple_robot/velocity',
+            self.listener_callback,
+            10)
+        #Create a publisher sending the vector as a message
+        self.publisher_ = self.create_publisher(Float64, '/dof1/reference_angles', 10)
+        # timer_period = 0.05  # seconds
+        # self.timer = self.create_timer(timer_period, self.ref_point_publisher)
+        # self.phi_start = 0.0
+        # self.phi_end = 1.0
+        # self.phi = self.phi_start
+        # self.delta = 0.005
+        # # List the points
+
+    def listener_callback(self, msg):
+        velocity = msg.data
+        timer_period = 1 / velocity  # seconds
+        self.timer = self.create_timer(timer_period, self.ref_point_publisher)
+        self.phi_start = 0.0
+        self.phi_end = 1.0
+        self.phi = self.phi_start
+        self.delta = 0.005
+        # List the points
+
+        
+    def ref_point_publisher(self):
+        # Ref position over the time
+        # Interpolate between x_0 - x_1
+               
+        msg = Float64()
+        self.phi = self.phi + self.delta       
+        
+        #setting back to 0
+        if self.phi>self.phi_end:
+            self.phi = self.phi_start
+        #self.get_logger().info('Resetting "%f"' % self.phi)
+
+        msg.data = self.phi
+        self.publisher_.publish(msg)
+        
+
+def main(args = None):
+    rclpy.init(args = args)
+    node = Reference_Node_1DoF_Velocity()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main() 
\ No newline at end of file