Improve launch files and 3dof simulation

ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_sequential_launch.py → ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_launch.py

@@ -2,6 +2,7 @@ import os
 from launch import LaunchDescription
 from launch_ros.actions import Node
 from launch.actions import ExecuteProcess, DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
 from ament_index_python.packages import get_package_share_directory
 from launch.substitutions import LaunchConfiguration
 
@@ -15,69 +16,102 @@ def generate_launch_description():
         'forward_kinematics_simulations.rviz'
     )
 
-    velocity_launch_arg = DeclareLaunchArgument('velocity', default_value = '0.015')
-    velocity = LaunchConfiguration('velocity')
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'max_velocity',
+            default_value = '0.015',
+            )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'acceleration',
+            default_value = '0.005',
+            )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'decelaration',
+            default_value = '0.005',
+            )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'velocity_constant',
+            default_value = '0.015',
+            )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'simultaneous',
+            default_value = 'True'
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'constant_speed',
+            default_value = 'False',
+            )
+    )
 
-    acceleration_launch_arg = DeclareLaunchArgument('acceleration', default_value = '0.0004')
+    max_velocity = LaunchConfiguration('max_velocity')
     acceleration = LaunchConfiguration('acceleration')
-
-    decelaration_launch_arg = DeclareLaunchArgument('decelaration', default_value = '0.0008')
     decelaration = LaunchConfiguration('decelaration')
-
-    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 = 'False')
+    simultaneous = LaunchConfiguration('simultaneous')
     constant_speed = LaunchConfiguration('constant_speed')
 
-    return LaunchDescription([
-        velocity_launch_arg,
-        acceleration_launch_arg,
-        decelaration_launch_arg,
-        slope_launch_arg,
-        velocity_constant_launch_arg,
-        constant_speed_launch_arg,
-        sequential_launch_arg,
-        Node(
+    forward_kinematics_node_1 = Node(
+            package = package_name,
+            executable = 'forward_kinematics_node_3dof_1',
+            name =  'forward_kinematics_node',
+            output = 'screen',
+            parameters = [],
+            condition = IfCondition(simultaneous)
+        )
+    forward_kinematics_node_2 = Node(
             package = package_name,
             executable = 'forward_kinematics_node_3dof_2',
             name =  'forward_kinematics_node',
             output = 'screen',
-            parameters = []
-        ),
-        Node(
+            parameters = [],
+            condition = UnlessCondition(simultaneous)
+        )
+    reference_node = Node(
             package = package_name,
             executable = 'reference_node_3dof',
             name =  'reference_node',
             output = 'screen',
             parameters = [
-                {'velocity': velocity},
+                {'max_velocity': max_velocity},
                 {'acceleration': acceleration},
                 {'decelaration': decelaration},
                 {'velocity_constant': velocity_constant},
-                {'slope': slope},
-                {'sequential': sequential},
-                {'constant_speed': constant_speed}
+                {'constant_speed': constant_speed},
+                {'simultaneous': simultaneous}
             ]
-        ),
-        Node(
+        )
+    rviz_node = Node(
             package='rviz2',
             executable='rviz2',
             name='rviz2',
             output='screen',
             arguments=['-d', rviz_config_file]
-        ),
-        Node(
+        )
+    inverse_kinematics_node = Node(
             package= 'iiwa_inverse_kinematics',
             executable='inverse_kinematics_node',
             name='inverse_kinematics',
             output='screen',
             parameters = []
         )
-    ])
\ No newline at end of file
+    
+    nodes = [
+        forward_kinematics_node_1,
+        forward_kinematics_node_2,
+        reference_node,
+        rviz_node,
+        inverse_kinematics_node
+    ]
+
+    return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file

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, DeclareLaunchArgument
-from ament_index_python.packages import get_package_share_directory
-from launch.substitutions import LaunchConfiguration
-
-def generate_launch_description():
-
-    package_name = 'simple_robot_3dof'
-
-    rviz_config_file = os.path.join(
-        get_package_share_directory(package_name),
-        'rviz',
-        'forward_kinematics_simulations.rviz'
-    )
-
-    velocity_launch_arg = DeclareLaunchArgument('velocity', default_value = '0.015')
-    velocity = LaunchConfiguration('velocity')
-
-    acceleration_launch_arg = DeclareLaunchArgument('acceleration', default_value = '0.0004')
-    acceleration = LaunchConfiguration('acceleration')
-
-    decelaration_launch_arg = DeclareLaunchArgument('decelaration', default_value = '0.0008')
-    decelaration = LaunchConfiguration('decelaration')
-
-    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 = 'False')
-    constant_speed = LaunchConfiguration('constant_speed')
-
-    return LaunchDescription([
-        velocity_launch_arg,
-        acceleration_launch_arg,
-        decelaration_launch_arg,
-        slope_launch_arg,
-        velocity_constant_launch_arg,
-        constant_speed_launch_arg,
-        sequential_launch_arg,
-        Node(
-            package = package_name,
-            executable = 'forward_kinematics_node_3dof_1',
-            name =  'forward_kinematics_node',
-            output = 'screen',
-            parameters = []
-        ),
-        Node(
-            package = package_name,
-            executable = 'reference_node_3dof',
-            name =  'reference_node',
-            output = 'screen',
-            parameters = [
-                {'velocity': velocity},
-                {'acceleration': acceleration},
-                {'decelaration': decelaration},
-                {'velocity_constant': velocity_constant},
-                {'slope': slope},
-                {'sequential': sequential},
-                {'constant_speed': constant_speed}
-            ]
-        ),
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            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/setup.py

@@ -10,8 +10,7 @@ setup(
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
-        ('share/' + package_name + '/launch', ['launch/kuka_rviz_simulation_3dof_sequential_launch.py']),
-        ('share/' + package_name + '/launch', ['launch/kuka_rviz_simulation_3dof_simultaneous_launch.py']),
+        ('share/' + package_name + '/launch', ['launch/kuka_rviz_simulation_3dof_launch.py']),
         ('share/' + package_name + '/rviz', ['rviz/forward_kinematics_simulations.rviz']),
     ],
     install_requires=['setuptools'],
@@ -23,8 +22,6 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            #'publisher = simple_robot_1dof.reference_node_1dof:main',
-            #'subscriber = simple_robot_1dof.forward_kinematics_node_1dof:main',
             'forward_kinematics_node_3dof_1 = simple_robot_3dof.forward_kinematics_node_3dof_1:main',
             'forward_kinematics_node_3dof_2 = simple_robot_3dof.forward_kinematics_node_3dof_2:main',
             'forward_kinematics_node_3dof_3 = simple_robot_3dof.forward_kinematics_node_3dof_3:main',

ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py

@@ -59,7 +59,7 @@ class Forward_Kinematics_Node_3Dof(Node):
         
     def transformation(self):
         # Define the desired rotation 
-        theta = pi / 2
+        theta =  - pi / 2
         # Split theta
         theta_fraction = self.phi * theta
 
@@ -82,6 +82,7 @@ class Forward_Kinematics_Node_3Dof(Node):
         ])
 
         self.joint_angles = np.zeros(len(self.Slist[0]))
+        self.joint_angles[0] = np.pi
         self.joint_angles[1] = np.pi/2
         self.joint_angles[3] = np.pi/2
         self.joint_angles[5] = np.pi/2

ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py

@@ -68,7 +68,7 @@ class Forward_Kinematics_Node_3Dof(Node):
         
     def rotation(self):
         # define the desired rotation 
-        theta = pi / 2
+        theta =  - pi / 2
         # split theta
         theta_fraction = self.phi * theta
 
@@ -125,6 +125,7 @@ class Forward_Kinematics_Node_3Dof(Node):
         ])
 
         self.joint_angles = np.zeros(len(self.Slist[0]))
+        self.joint_angles[0] = np.pi
         self.joint_angles[1] = np.pi/2
         self.joint_angles[3] = np.pi/2
         self.joint_angles[5] = np.pi/2

ros2_ws/src/simple_robot_3dof/simple_robot_3dof/reference_node_3dof.py

@@ -13,32 +13,32 @@ class Reference_Node_3DoF(Node):
         #Create a publisher sending the vector as a message
         self.publisher_ = self.create_publisher(Float64, '/dof3/reference_angles', 10)
 
-        self.declare_parameter('velocity', 0.0)
+        self.declare_parameter('max_velocity', 0.0)
         self.declare_parameter('acceleration', 0.0)
         self.declare_parameter('decelaration', 0.0)
         self.declare_parameter('velocity_constant', 0.0)
-        self.declare_parameter('slope', 0.0)
         self.declare_parameter('constant_speed', False)
-        self.declare_parameter('sequential', False)
+        self.declare_parameter('simultaneous', True)
 
-        self.velocity = self.get_parameter('velocity').get_parameter_value().double_value
+        self.max_velocity = self.get_parameter('max_velocity').get_parameter_value().double_value
         self.acceleration = self.get_parameter('acceleration').get_parameter_value().double_value
         self.decelaration = self.get_parameter('decelaration').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.constant_speed = self.get_parameter('constant_speed').get_parameter_value().bool_value
-        self.sequential = self.get_parameter('sequential').get_parameter_value().bool_value
+        self.simultaneous = self.get_parameter('simultaneous').get_parameter_value().bool_value
 
 
         self.phi_start = 0.0
         self.phi_end = 1.0
         self.phi = self.phi_start
-        self.velocity_start = self.velocity
+        self.transition_point = 0.0
 
         self.index = 0
 
         self.timer_period = 0.1  # seconds
         self.timer = self.create_timer(self.timer_period, self.ref_point_publisher)
+
+        self.current_velocity = self.timer_period * self.acceleration
         # List the points
         
     def ref_point_publisher(self):
@@ -47,39 +47,41 @@ class Reference_Node_3DoF(Node):
                
         msg = Float64()
 
-        if self.sequential == False:
+        if self.simultaneous:
             if self.constant_speed == True:
                self.phi = self.phi + self.velocity_constant  
             elif self.constant_speed == False:
-               if self.phi < self.slope:
-                   self.phi = self.phi + self.velocity
-                   self.velocity = self.velocity + self.acceleration
-               elif self.phi > 1 - self.slope:
-                   self.phi = self.phi + self.velocity
-                   self.velocity = self.velocity - self.decelaration
+               if self.current_velocity < self.max_velocity and self.phi < 0.5:
+                   self.phi = self.phi + self.current_velocity
+                   self.current_velocity = self.current_velocity + self.timer_period * self.acceleration
+                   self.transition_point = self.phi
+               elif self.phi > 1 - self.transition_point:
+                   self.phi = self.phi + self.current_velocity
+                   self.current_velocity = self.current_velocity - self.timer_period * self.decelaration
                else:
-                   self.phi = self.phi + self.velocity
-        elif self.sequential == True:
+                   self.phi = self.phi + self.current_velocity
+        else:
             if self.constant_speed == True:
-                self.phi = self.phi + self.velocity_constant 
+                self.phi = self.phi + self.current_velocity 
             elif self.constant_speed == False:
                 if self.index % 2 == 0:
                     self.phi = self.phi + self.velocity_constant 
                 else:
-                    if self.phi < self.slope:
-                        self.phi = self.phi + self.velocity
-                        self.velocity = self.velocity + self.acceleration
-                    elif self.phi > 1 - self.slope:
-                        self.phi = self.phi + self.velocity
-                        self.velocity = self.velocity - self.decelaration
+                    if self.current_velocity < self.max_velocity:
+                        self.phi = self.phi + self.current_velocity
+                        self.current_velocity = self.current_velocity + self.timer_period * self.acceleration
+                        self.transition_point = self.phi
+                    elif self.phi > 1 - self.transition_point:
+                        self.phi = self.phi + self.current_velocity
+                        self.current_velocity = self.current_velocity - self.timer_period * self.decelaration
                     else:
-                        self.phi = self.phi + self.velocity
+                        self.phi = self.phi + self.current_velocity
 
         #setting back to 0
         if self.phi>self.phi_end:
             self.phi = self.phi_start
             self.index += 1
-            self.velocity = self.velocity_start
+            self.current_velocity = self.timer_period * self.acceleration
             # self.get_logger().info('Resetting "%f"' % self.phi)
 
         msg.data = self.phi