Skip to content
Snippets Groups Projects
Commit 5f395a7f authored by Ventsislav Dokusanski's avatar Ventsislav Dokusanski
Browse files

Improve speed profiles of the 2dof simulation

parent 39cd0a66
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_sequential_launch.py
+ 12
− 12
View file @ 5f395a7f
......@@ -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},
......
ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_simultaneous_launch.py
+ 13
− 13
View file @ 5f395a7f
......@@ -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},
......
ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py
+ 24
− 17
View file @ 5f395a7f
......@@ -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
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment

Help | Imprint | Privacy policy | Accessibility | Contact