diff --git a/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_sequential_launch.py b/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_sequential_launch.py
index 23376438cba6f76bc9a6c2eb1bb188c945c750ea..0aab324de86879741eb1d0348a39cf7de13994b7 100644
--- a/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_sequential_launch.py
+++ b/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_sequential_launch.py
@@ -1,8 +1,9 @@
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',
diff --git a/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_simultaneous_launch.py b/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_simultaneous_launch.py
index 7ac02475cff001f52fbd7368672cd253d5f07958..8ce0d772f70b3411c0f20f4168c22cfb1738907b 100644
--- a/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_simultaneous_launch.py
+++ b/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_simultaneous_launch.py
@@ -1,8 +1,9 @@
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',
diff --git a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py
index 8fd47a9c82aa2688fc1ca66059b7459c1c9c8498..dc6548cdf13dbd3130b83e54aaa22916f1f4aeea 100644
--- a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py
+++ b/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)
diff --git a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py
index 2b1479a1bde989fae4e718ed624690722129417f..07c3c1de62956107f9a1401bdc22a9ec94bcb903 100644
--- a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py
+++ b/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)
diff --git a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_3.py b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_3.py
index 0062a17a7e720b5d3dfc3a98a9cc3349f01ef77a..82d6c5e3a1b7561493cfef1a04741c74c5e06f8a 100644
--- a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_3.py
+++ b/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,
diff --git a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/reference_node_3dof.py b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/reference_node_3dof.py
index 7300d478f5ba056600b7fd1b68766a355f612c70..3f60cedf3dff0d719f04b350436b8e572ec3de6f 100644
--- a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/reference_node_3dof.py
+++ b/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)