From 5f395a7f011802ae07b076a758fe11ddd5203995 Mon Sep 17 00:00:00 2001
From: Ventsi <uquzq@student.kit.edu>
Date: Tue, 23 Jul 2024 16:07:58 +0200
Subject: [PATCH] Improve speed profiles of the 2dof simulation
---
..._rviz_simulation_2dof_sequential_launch.py | 24 +++++------
...viz_simulation_2dof_simultaneous_launch.py | 26 ++++++------
.../simple_robot_2dof/reference_node_2dof.py | 41 +++++++++++--------
3 files changed, 49 insertions(+), 42 deletions(-)
diff --git a/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_sequential_launch.py b/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_sequential_launch.py
index b4a2686..7976973 100644
--- a/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_sequential_launch.py
+++ b/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_sequential_launch.py
@@ -15,14 +15,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')
+ 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')
@@ -37,10 +37,10 @@ def generate_launch_description():
constant_speed = LaunchConfiguration('constant_speed')
return LaunchDescription([
- velocity1_launch_arg,
- velocity2_launch_arg,
+ velocity_launch_arg,
+ acceleration_launch_arg,
slope_launch_arg,
- velocity3_launch_arg,
+ decelaration_launch_arg,
velocity_constant_launch_arg,
constant_speed_launch_arg,
sequential_launch_arg,
@@ -57,9 +57,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},
{'sequential': sequential},
diff --git a/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_simultaneous_launch.py b/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_simultaneous_launch.py
index 5112ac7..15ecf55 100644
--- a/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_simultaneous_launch.py
+++ b/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_simultaneous_launch.py
@@ -15,14 +15,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')
+ 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')
@@ -33,14 +33,14 @@ def generate_launch_description():
sequential_launch_arg = DeclareLaunchArgument('sequential', default_value = 'False') # do not change
sequential = LaunchConfiguration('sequential')
- constant_speed_launch_arg = DeclareLaunchArgument('constant_speed', default_value = 'False') # change
+ constant_speed_launch_arg = DeclareLaunchArgument('constant_speed', default_value = 'False')
constant_speed = LaunchConfiguration('constant_speed')
return LaunchDescription([
- velocity1_launch_arg,
- velocity2_launch_arg,
+ velocity_launch_arg,
+ acceleration_launch_arg,
slope_launch_arg,
- velocity3_launch_arg,
+ decelaration_launch_arg,
velocity_constant_launch_arg,
constant_speed_launch_arg,
sequential_launch_arg,
@@ -57,9 +57,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},
{'sequential': sequential},
diff --git a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py
index 88d3ad9..e71a96e 100644
--- a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py
+++ b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py
@@ -13,27 +13,27 @@ class Reference_Node_2DoF(Node):
#Create a publisher sending the vector as a message
self.publisher_ = self.create_publisher(Float64, '/dof2/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('sequential', False)
self.declare_parameter('constant_speed', False)
+ self.declare_parameter('sequential', 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.sequential = self.get_parameter('sequential').get_parameter_value().bool_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.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)
@@ -47,26 +47,33 @@ class Reference_Node_2DoF(Node):
if self.sequential == False:
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
elif self.phi > 1 - self.slope:
- self.phi = self.phi + self.velocity3
+ self.phi = self.phi + self.velocity
+ self.velocity = self.velocity - self.decelaration
else:
- self.phi = self.phi + self.velocity2
+ self.phi = self.phi + self.velocity
elif self.sequential == True:
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
+ elif self.phi > 1 - self.slope:
+ self.phi = self.phi + self.velocity
+ self.velocity = self.velocity - self.decelaration
else:
- self.phi = self.phi + self.velocity2
+ self.phi = self.phi + 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
--
GitLab