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 6a39c00706e54352e13cdc5ee51c7fe648630183..1327740c587bc816102b8910745845663155e28a 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
@@ -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')
+ 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_2dof_2',
@@ -27,7 +56,15 @@ def generate_launch_description():
executable = 'reference_node_2dof',
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_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 39badf8aadaf6cc489c28d3c3f0db7a0bf96764e..5112ac734ff7d59bb3bdc8241bdf882a2d677275 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
@@ -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 = 'False') # 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_2dof_1',
@@ -27,7 +56,15 @@ def generate_launch_description():
executable = 'reference_node_2dof',
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_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
index 1988ebce939d6e7acd84815a030f739c0f522709..7be6ee84c6e31a1bcbc5207edd04cad63aaec75e 100644
--- a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
+++ b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
@@ -19,7 +19,7 @@ import time
class Forward_Kinematics_Node_2Dof(Node):
def __init__(self):
- super().__init__('forward_kinematics_node_1dof')
+ super().__init__('forward_kinematics_node_2dof')
# Create a subscriber, initialising the transformation
self.subscription = self.create_subscription(
Float64,
@@ -50,9 +50,15 @@ class Forward_Kinematics_Node_2Dof(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()
@@ -95,7 +101,8 @@ class Forward_Kinematics_Node_2Dof(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)
@@ -177,6 +184,7 @@ class Forward_Kinematics_Node_2Dof(Node):
self.pose_publisher_kuka_robot_.publish(pose_msg)
# if self.phi == 0.0:
# time.sleep(1)
+
def main(args = None):
diff --git a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py
index e0fe2a98a5ff385bf956568f4014068dbce633a9..a605044e4832c5f88ac22cf247a67dcc36a99a39 100644
--- a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py
+++ b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py
@@ -19,7 +19,7 @@ import time
class Forward_Kinematics_Node_2Dof(Node):
def __init__(self):
- super().__init__('forward_kinematics_node_1dof')
+ super().__init__('forward_kinematics_node_2dof')
# Create a subscriber, initialising the transformation
self.subscription = self.create_subscription(
Float64,
@@ -54,7 +54,7 @@ class Forward_Kinematics_Node_2Dof(Node):
self.phi = msg.data
# Define index and if phi is resetted add 1 to the index
if(self.phi == 0):
- self.index +=1
+ self.index += 1
# check which function to call
if(self.index % 2 == 0):
@@ -103,7 +103,9 @@ class Forward_Kinematics_Node_2Dof(Node):
self.end_vector = np.matmul(rotation, 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 rotation
self.construct_path(end_position_1)
@@ -127,7 +129,10 @@ class Forward_Kinematics_Node_2Dof(Node):
end_position_2 = end_forward_kinematics_2[:4, 3]
end_orientation_2 = Rotation.from_matrix(end_forward_kinematics_2[:3, :3]).as_quat()
- self.publish_pose_target_pose(end_position_2, end_orientation_2)
+ self.publish_pose_simple_robot(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 translation
self.construct_path(end_position_2)
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 ae03518bafdf57b9b83d308bb19fa8eed34b86db..0c3482d0a7e304824a9dd062de9dca7cde61ce93 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
@@ -9,15 +9,34 @@ import time
class Reference_Node_2DoF(Node):
def __init__(self):
- super().__init__('reference_node_1dof')
+ super().__init__('reference_node_2dof')
#Create a publisher sending the vector as a message
- self.publisher_ = self.create_publisher(Float64, '/dof2/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, '/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_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.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 +44,42 @@ class Reference_Node_2DoF(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.phi < self.slope:
+ self.phi = self.phi + self.velocity1
+ else:
+ self.phi = self.phi + self.velocity2
+
+
+
+ # 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
+
#setting back to 0
if self.phi>self.phi_end:
self.phi = self.phi_start
- #self.get_logger().info('Resetting "%f"' % self.phi)
+ # self.get_logger().info('Resetting "%f"' % self.phi)
msg.data = self.phi
self.publisher_.publish(msg)