Create velocity profiles for the 3 dof movement

ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_sequential_launch.py

 import os
 from launch import LaunchDescription
 from launch_ros.actions import Node
-from launch.actions import ExecuteProcess
+from launch.actions import ExecuteProcess, DeclareLaunchArgument
 from ament_index_python.packages import get_package_share_directory
+from launch.substitutions import LaunchConfiguration
 
 def generate_launch_description():
 
@@ -14,7 +15,35 @@ def generate_launch_description():
         'forward_kinematics_simulations.rviz'
     )
 
+    velocity1_launch_arg = DeclareLaunchArgument('velocity1', default_value = '0.01') # constant: 0.015 acceleration: 0.005
+    velocity1 = LaunchConfiguration('velocity1')
+
+    velocity2_launch_arg = DeclareLaunchArgument('velocity2', default_value = '0.03')
+    velocity2 = LaunchConfiguration('velocity2')
+
+    velocity3_launch_arg = DeclareLaunchArgument('velocity3', default_value = '0.005')
+    velocity3 = LaunchConfiguration('velocity3')
+
+    slope_launch_arg = DeclareLaunchArgument('slope', default_value = '0.25') # [0 ; 0.5]
+    slope = LaunchConfiguration('slope')
+
+    velocity_constant_launch_arg = DeclareLaunchArgument('velocity_constant', default_value = '0.015')
+    velocity_constant = LaunchConfiguration('velocity_constant')
+
+    sequential_launch_arg = DeclareLaunchArgument('sequential', default_value = 'True') # do not change
+    sequential = LaunchConfiguration('sequential')
+
+    constant_speed_launch_arg = DeclareLaunchArgument('constant_speed', default_value = 'True') # change
+    constant_speed = LaunchConfiguration('constant_speed')
+
     return LaunchDescription([
+        velocity1_launch_arg,
+        velocity2_launch_arg,
+        slope_launch_arg,
+        velocity3_launch_arg,
+        velocity_constant_launch_arg,
+        constant_speed_launch_arg,
+        sequential_launch_arg,
         Node(
             package = package_name,
             executable = 'forward_kinematics_node_3dof_2',
@@ -27,7 +56,15 @@ def generate_launch_description():
             executable = 'reference_node_3dof',
             name =  'reference_node',
             output = 'screen',
-            parameters = []
+            parameters = [
+                {'velocity1': velocity1},
+                {'velocity2': velocity2},
+                {'velocity3': velocity3},
+                {'velocity_constant': velocity_constant},
+                {'slope': slope},
+                {'sequential': sequential},
+                {'constant_speed': constant_speed}
+            ]
         ),
         Node(
             package='rviz2',

ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_simultaneous_launch.py

 import os
 from launch import LaunchDescription
 from launch_ros.actions import Node
-from launch.actions import ExecuteProcess
+from launch.actions import ExecuteProcess, DeclareLaunchArgument
 from ament_index_python.packages import get_package_share_directory
+from launch.substitutions import LaunchConfiguration
 
 def generate_launch_description():
 
@@ -14,7 +15,35 @@ def generate_launch_description():
         'forward_kinematics_simulations.rviz'
     )
 
+    velocity1_launch_arg = DeclareLaunchArgument('velocity1', default_value = '0.01') # constant: 0.015 acceleration: 0.005
+    velocity1 = LaunchConfiguration('velocity1')
+
+    velocity2_launch_arg = DeclareLaunchArgument('velocity2', default_value = '0.03')
+    velocity2 = LaunchConfiguration('velocity2')
+
+    velocity3_launch_arg = DeclareLaunchArgument('velocity3', default_value = '0.005')
+    velocity3 = LaunchConfiguration('velocity3')
+
+    slope_launch_arg = DeclareLaunchArgument('slope', default_value = '0.25') # [0 ; 0.5]
+    slope = LaunchConfiguration('slope')
+
+    velocity_constant_launch_arg = DeclareLaunchArgument('velocity_constant', default_value = '0.015')
+    velocity_constant = LaunchConfiguration('velocity_constant')
+
+    sequential_launch_arg = DeclareLaunchArgument('sequential', default_value = 'False') # do not change
+    sequential = LaunchConfiguration('sequential')
+
+    constant_speed_launch_arg = DeclareLaunchArgument('constant_speed', default_value = 'True') # change
+    constant_speed = LaunchConfiguration('constant_speed')
+
     return LaunchDescription([
+        velocity1_launch_arg,
+        velocity2_launch_arg,
+        slope_launch_arg,
+        velocity3_launch_arg,
+        velocity_constant_launch_arg,
+        constant_speed_launch_arg,
+        sequential_launch_arg,
         Node(
             package = package_name,
             executable = 'forward_kinematics_node_3dof_1',
@@ -27,7 +56,15 @@ def generate_launch_description():
             executable = 'reference_node_3dof',
             name =  'reference_node',
             output = 'screen',
-            parameters = []
+            parameters = [
+                {'velocity1': velocity1},
+                {'velocity2': velocity2},
+                {'velocity3': velocity3},
+                {'velocity_constant': velocity_constant},
+                {'slope': slope},
+                {'sequential': sequential},
+                {'constant_speed': constant_speed}
+            ]
         ),
         Node(
             package='rviz2',

ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py

@@ -18,7 +18,7 @@ import time
 class Forward_Kinematics_Node_3Dof(Node):
 
     def __init__(self):
-        super().__init__('forward_kinematics_node_1dof')
+        super().__init__('forward_kinematics_node_3dof')
         # Create a subscriber, initialising the transformation
         self.subscription = self.create_subscription(
             Float64,
@@ -46,9 +46,14 @@ class Forward_Kinematics_Node_3Dof(Node):
         self.path.header.frame_id = "map"
         self.path.header.stamp = self.get_clock().now().to_msg()
 
+        self.index = 0
+
     def listener_callback(self, msg):
         # Get the message
         self.phi = msg.data
+
+        if self.phi == 0:
+            self.index += 1
         # call the transformation function
         self.transformation()
         
@@ -92,7 +97,8 @@ class Forward_Kinematics_Node_3Dof(Node):
         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)
+        if self.index == 1:
+            self.publish_pose_target_pose(end_position, end_orientation)
 
         self.construct_path(end_position)
 

ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py

@@ -19,7 +19,7 @@ import time
 class Forward_Kinematics_Node_3Dof(Node):
 
     def __init__(self):
-        super().__init__('forward_kinematics_node_1dof')
+        super().__init__('forward_kinematics_node_3dof')
         # Create a subscriber, initialising the transformation
         self.subscription = self.create_subscription(
             Float64,
@@ -88,7 +88,8 @@ class Forward_Kinematics_Node_3Dof(Node):
         end_orientation_2 = Rotation.from_matrix(end_forward_kinematics_2[:3, :3]).as_quat()
 
         self.publish_pose_simple_robot(end_position_2, end_orientation_2)
-        self.publish_pose_target_pose(end_position_2, end_orientation_2)
+        if self.index == 3:
+            self.publish_pose_target_pose(end_position_2, end_orientation_2)
 
         # construct the path of the rotation
         self.construct_path(end_position_2)
@@ -142,7 +143,8 @@ class Forward_Kinematics_Node_3Dof(Node):
         self.end_vector = np.matmul(translation_matrix, start_position)
 
         self.publish_pose_simple_robot(end_position_1, end_orientation_1)
-        self.publish_pose_target_pose(end_position_1, end_orientation_1)
+        if self.index == 2:
+            self.publish_pose_target_pose(end_position_1, end_orientation_1)
 
         # construct the path of the translation
         self.construct_path(end_position_1)

ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_3.py

@@ -19,7 +19,7 @@ import time
 class Forward_Kinematics_Node_3Dof(Node):
 
     def __init__(self):
-        super().__init__('forward_kinematics_node_1dof')
+        super().__init__('forward_kinematics_node_3dof')
         # Create a subscriber, initialising the transformation
         self.subscription = self.create_subscription(
             Float64,

ros2_ws/src/simple_robot_3dof/simple_robot_3dof/reference_node_3dof.py

@@ -11,13 +11,33 @@ class Reference_Node_3DoF(Node):
     def __init__(self):
         super().__init__('reference_node_1dof')
         #Create a publisher sending the vector as a message
-        self.publisher_ = self.create_publisher(Float64, '/dof3/reference_angles', 10)#
-        timer_period = 0.05  # seconds
-        self.timer = self.create_timer(timer_period, self.ref_point_publisher)
+        self.publisher_ = self.create_publisher(Float64, '/dof3/reference_angles', 10)
+
+        self.declare_parameter('velocity1', 0.0)
+        self.declare_parameter('velocity2', 0.0)
+        self.declare_parameter('velocity3', 0.0)
+        self.declare_parameter('velocity_constant', 0.0)
+        self.declare_parameter('slope', 0.0)
+        self.declare_parameter('sequential', False)
+        self.declare_parameter('constant_speed', False)
+
+        self.velocity1 = self.get_parameter('velocity1').get_parameter_value().double_value
+        self.velocity2 = self.get_parameter('velocity2').get_parameter_value().double_value
+        self.velocity3 = self.get_parameter('velocity3').get_parameter_value().double_value
+        self.velocity_constant = self.get_parameter('velocity_constant').get_parameter_value().double_value
+        self.slope = self.get_parameter('slope').get_parameter_value().double_value
+        self.sequential = self.get_parameter('sequential').get_parameter_value().bool_value
+        self.constant_speed = self.get_parameter('constant_speed').get_parameter_value().bool_value
+
         self.phi_start = 0.0
         self.phi_end = 1.0
         self.phi = self.phi_start
-        self.delta = 0.005
+        self.delta = self.velocity_constant
+
+        self.index = 0
+
+        self.timer_period = 0.1  # seconds
+        self.timer = self.create_timer(self.timer_period, self.ref_point_publisher)
         # List the points
         
     def ref_point_publisher(self):
@@ -25,12 +45,36 @@ class Reference_Node_3DoF(Node):
         # Interpolate between x_0 - x_1
                
         msg = Float64()
-        self.phi = self.phi + self.delta       
-        
+
+        if self.sequential == False:
+            if self.constant_speed == True:
+               self.phi = self.phi + self.delta  
+            elif self.constant_speed == False:
+               if self.phi < self.slope:
+                   self.phi = self.phi + self.velocity1
+               elif self.phi > 1 - self.slope:
+                   self.phi = self.phi + self.velocity3
+               else:
+                   self.phi = self.phi + self.velocity2
+        elif self.sequential == True:
+            if self.constant_speed == True:
+                self.phi = self.phi + self.delta 
+            elif self.constant_speed == False:
+                if self.index % 2 == 0:
+                    self.phi = self.phi + self.delta 
+                else:
+                    if self.phi < self.slope:
+                        self.phi = self.phi + self.velocity1
+                    elif self.phi > 1 - self.slope:
+                        self.phi = self.phi + self.velocity3
+                    else:
+                        self.phi = self.phi + self.velocity2
+
         #setting back to 0
         if self.phi>self.phi_end:
             self.phi = self.phi_start
-        #self.get_logger().info('Resetting "%f"' % self.phi)
+            self.index += 1
+            # self.get_logger().info('Resetting "%f"' % self.phi)
 
         msg.data = self.phi
         self.publisher_.publish(msg)