Improve speed profile of the 1dof simulation

ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py

@@ -16,14 +16,14 @@ 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')
+    velocity_launch_arg = DeclareLaunchArgument('velocity', default_value = '0.015') # constant: 0.015 acceleration: 0.005
+    velocity = LaunchConfiguration('velocity')
 
-    velocity2_launch_arg = DeclareLaunchArgument('velocity2', default_value = '0.03')
-    velocity2 = LaunchConfiguration('velocity2')
+    acceleration_launch_arg = DeclareLaunchArgument('acceleration', default_value = '0.0004')
+    acceleration = LaunchConfiguration('acceleration')
 
-    velocity3_launch_arg = DeclareLaunchArgument('velocity3', default_value = '0.005')
-    velocity3 = LaunchConfiguration('velocity3')
+    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')
@@ -36,10 +36,10 @@ def generate_launch_description():
 
 
     return LaunchDescription([
-        velocity1_launch_arg,
-        velocity2_launch_arg,
+        velocity_launch_arg,
+        acceleration_launch_arg,
+        decelaration_launch_arg,
         slope_launch_arg,
-        velocity3_launch_arg,
         velocity_constant_launch_arg,
         constant_speed_launch_arg,
         Node(
@@ -55,9 +55,9 @@ def generate_launch_description():
             name =  'reference_node',
             output = 'screen',
             parameters = [
-                {'velocity1': velocity1},
-                {'velocity2': velocity2},
-                {'velocity3': velocity3},
+                {'velocity': velocity},
+                {'acceleration': acceleration},
+                {'decelaration': decelaration},
                 {'velocity_constant': velocity_constant},
                 {'slope': slope},
                 {'constant_speed': constant_speed}

ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py

@@ -13,16 +13,16 @@ class Reference_Node_1DoF(Node):
         #Create a publisher sending the vector as a message
         self.publisher_ = self.create_publisher(Float64, '/dof1/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', 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.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 = self.get_parameter('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
@@ -30,7 +30,7 @@ class Reference_Node_1DoF(Node):
         self.phi_start = 0.0
         self.phi_end = 1.0
         self.phi = self.phi_start
-        self.delta = self.velocity_constant
+        self.velocity_start = self.velocity
 
         self.timer_period = 0.1  # seconds
         self.timer = self.create_timer(self.timer_period, self.ref_point_publisher)
@@ -43,18 +43,27 @@ class Reference_Node_1DoF(Node):
         msg = Float64()
 
         if self.constant_speed == True:
-            self.phi = self.phi + self.delta  
+            self.phi = self.phi + self.velocity_constant  
         elif self.constant_speed == False:
             if self.phi < self.slope:
-                self.phi = self.phi + self.velocity1
+                self.phi = self.phi + self.velocity
+                self.velocity = self.velocity + self.acceleration
+                self.get_logger().info('Acceleration phase')
+                self.get_logger().info('%f' % self.velocity)
             elif self.phi > 1 - self.slope:
-                self.phi = self.phi + self.velocity3
+                self.phi = self.phi + self.velocity
+                self.velocity = self.velocity - self.decelaration
+                self.get_logger().info('Decelaration phase')
+                self.get_logger().info('%f' % self.velocity)
             else:
-                self.phi = self.phi + self.velocity2
+                self.phi = self.phi + self.velocity
+                self.get_logger().info('Constant speed phase')
+                self.get_logger().info('%f' % self.velocity)
 
         #setting back to 0
         if self.phi>self.phi_end:
             self.phi = self.phi_start
+            self.velocity = self.velocity_start
             # self.get_logger().info('Resetting "%f"' % self.phi)
 
         msg.data = self.phi