From eced627a29d1076dc22c9b37696b072611f95285 Mon Sep 17 00:00:00 2001
From: Ventsi <uquzq@student.kit.edu>
Date: Thu, 25 Jul 2024 19:33:56 +0200
Subject: [PATCH] Improve launch files and 3dof simulation
---
.../kuka_rviz_simulation_3dof_launch.py | 117 ++++++++++++++++++
..._rviz_simulation_3dof_sequential_launch.py | 83 -------------
...viz_simulation_3dof_simultaneous_launch.py | 83 -------------
ros2_ws/src/simple_robot_3dof/setup.py | 5 +-
.../forward_kinematics_node_3dof_1.py | 3 +-
.../forward_kinematics_node_3dof_2.py | 3 +-
.../simple_robot_3dof/reference_node_3dof.py | 52 ++++----
7 files changed, 149 insertions(+), 197 deletions(-)
create mode 100644 ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_launch.py
delete mode 100644 ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_sequential_launch.py
delete mode 100644 ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_simultaneous_launch.py
diff --git a/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_launch.py b/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_launch.py
new file mode 100644
index 0000000..02febc5
--- /dev/null
+++ b/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_launch.py
@@ -0,0 +1,117 @@
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess, DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from ament_index_python.packages import get_package_share_directory
+from launch.substitutions import LaunchConfiguration
+
+def generate_launch_description():
+
+ package_name = 'simple_robot_3dof'
+
+ rviz_config_file = os.path.join(
+ get_package_share_directory(package_name),
+ 'rviz',
+ 'forward_kinematics_simulations.rviz'
+ )
+
+ declared_arguments = []
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ 'max_velocity',
+ default_value = '0.015',
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ 'acceleration',
+ default_value = '0.005',
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ 'decelaration',
+ default_value = '0.005',
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ 'velocity_constant',
+ default_value = '0.015',
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ 'simultaneous',
+ default_value = 'True'
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ 'constant_speed',
+ default_value = 'False',
+ )
+ )
+
+ max_velocity = LaunchConfiguration('max_velocity')
+ acceleration = LaunchConfiguration('acceleration')
+ decelaration = LaunchConfiguration('decelaration')
+ velocity_constant = LaunchConfiguration('velocity_constant')
+ simultaneous = LaunchConfiguration('simultaneous')
+ constant_speed = LaunchConfiguration('constant_speed')
+
+ forward_kinematics_node_1 = Node(
+ package = package_name,
+ executable = 'forward_kinematics_node_3dof_1',
+ name = 'forward_kinematics_node',
+ output = 'screen',
+ parameters = [],
+ condition = IfCondition(simultaneous)
+ )
+ forward_kinematics_node_2 = Node(
+ package = package_name,
+ executable = 'forward_kinematics_node_3dof_2',
+ name = 'forward_kinematics_node',
+ output = 'screen',
+ parameters = [],
+ condition = UnlessCondition(simultaneous)
+ )
+ reference_node = Node(
+ package = package_name,
+ executable = 'reference_node_3dof',
+ name = 'reference_node',
+ output = 'screen',
+ parameters = [
+ {'max_velocity': max_velocity},
+ {'acceleration': acceleration},
+ {'decelaration': decelaration},
+ {'velocity_constant': velocity_constant},
+ {'constant_speed': constant_speed},
+ {'simultaneous': simultaneous}
+ ]
+ )
+ rviz_node = Node(
+ package='rviz2',
+ executable='rviz2',
+ name='rviz2',
+ output='screen',
+ arguments=['-d', rviz_config_file]
+ )
+ inverse_kinematics_node = Node(
+ package= 'iiwa_inverse_kinematics',
+ executable='inverse_kinematics_node',
+ name='inverse_kinematics',
+ output='screen',
+ parameters = []
+ )
+
+ nodes = [
+ forward_kinematics_node_1,
+ forward_kinematics_node_2,
+ reference_node,
+ rviz_node,
+ inverse_kinematics_node
+ ]
+
+ return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
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
deleted file mode 100644
index 7553b5e..0000000
--- a/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_sequential_launch.py
+++ /dev/null
@@ -1,83 +0,0 @@
-import os
-from launch import LaunchDescription
-from launch_ros.actions import Node
-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():
-
- package_name = 'simple_robot_3dof'
-
- rviz_config_file = os.path.join(
- get_package_share_directory(package_name),
- 'rviz',
- 'forward_kinematics_simulations.rviz'
- )
-
- velocity_launch_arg = DeclareLaunchArgument('velocity', default_value = '0.015')
- velocity = LaunchConfiguration('velocity')
-
- acceleration_launch_arg = DeclareLaunchArgument('acceleration', default_value = '0.0004')
- acceleration = LaunchConfiguration('acceleration')
-
- 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')
-
- 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 = 'False')
- constant_speed = LaunchConfiguration('constant_speed')
-
- return LaunchDescription([
- velocity_launch_arg,
- acceleration_launch_arg,
- decelaration_launch_arg,
- slope_launch_arg,
- velocity_constant_launch_arg,
- constant_speed_launch_arg,
- sequential_launch_arg,
- Node(
- package = package_name,
- executable = 'forward_kinematics_node_3dof_2',
- name = 'forward_kinematics_node',
- output = 'screen',
- parameters = []
- ),
- Node(
- package = package_name,
- executable = 'reference_node_3dof',
- name = 'reference_node',
- output = 'screen',
- parameters = [
- {'velocity': velocity},
- {'acceleration': acceleration},
- {'decelaration': decelaration},
- {'velocity_constant': velocity_constant},
- {'slope': slope},
- {'sequential': sequential},
- {'constant_speed': constant_speed}
- ]
- ),
- Node(
- package='rviz2',
- executable='rviz2',
- name='rviz2',
- output='screen',
- arguments=['-d', rviz_config_file]
- ),
- Node(
- package= 'iiwa_inverse_kinematics',
- executable='inverse_kinematics_node',
- name='inverse_kinematics',
- output='screen',
- parameters = []
- )
- ])
\ No newline at end of file
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
deleted file mode 100644
index 4febcff..0000000
--- a/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_simultaneous_launch.py
+++ /dev/null
@@ -1,83 +0,0 @@
-import os
-from launch import LaunchDescription
-from launch_ros.actions import Node
-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():
-
- package_name = 'simple_robot_3dof'
-
- rviz_config_file = os.path.join(
- get_package_share_directory(package_name),
- 'rviz',
- 'forward_kinematics_simulations.rviz'
- )
-
- velocity_launch_arg = DeclareLaunchArgument('velocity', default_value = '0.015')
- velocity = LaunchConfiguration('velocity')
-
- acceleration_launch_arg = DeclareLaunchArgument('acceleration', default_value = '0.0004')
- acceleration = LaunchConfiguration('acceleration')
-
- 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')
-
- 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')
- constant_speed = LaunchConfiguration('constant_speed')
-
- return LaunchDescription([
- velocity_launch_arg,
- acceleration_launch_arg,
- decelaration_launch_arg,
- slope_launch_arg,
- velocity_constant_launch_arg,
- constant_speed_launch_arg,
- sequential_launch_arg,
- Node(
- package = package_name,
- executable = 'forward_kinematics_node_3dof_1',
- name = 'forward_kinematics_node',
- output = 'screen',
- parameters = []
- ),
- Node(
- package = package_name,
- executable = 'reference_node_3dof',
- name = 'reference_node',
- output = 'screen',
- parameters = [
- {'velocity': velocity},
- {'acceleration': acceleration},
- {'decelaration': decelaration},
- {'velocity_constant': velocity_constant},
- {'slope': slope},
- {'sequential': sequential},
- {'constant_speed': constant_speed}
- ]
- ),
- Node(
- package='rviz2',
- executable='rviz2',
- name='rviz2',
- output='screen',
- arguments=['-d', rviz_config_file]
- ),
- Node(
- package= 'iiwa_inverse_kinematics',
- executable='inverse_kinematics_node',
- name='inverse_kinematics',
- output='screen',
- parameters = []
- )
- ])
\ No newline at end of file
diff --git a/ros2_ws/src/simple_robot_3dof/setup.py b/ros2_ws/src/simple_robot_3dof/setup.py
index 25b8c83..11f228e 100644
--- a/ros2_ws/src/simple_robot_3dof/setup.py
+++ b/ros2_ws/src/simple_robot_3dof/setup.py
@@ -10,8 +10,7 @@ setup(
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
- ('share/' + package_name + '/launch', ['launch/kuka_rviz_simulation_3dof_sequential_launch.py']),
- ('share/' + package_name + '/launch', ['launch/kuka_rviz_simulation_3dof_simultaneous_launch.py']),
+ ('share/' + package_name + '/launch', ['launch/kuka_rviz_simulation_3dof_launch.py']),
('share/' + package_name + '/rviz', ['rviz/forward_kinematics_simulations.rviz']),
],
install_requires=['setuptools'],
@@ -23,8 +22,6 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
- #'publisher = simple_robot_1dof.reference_node_1dof:main',
- #'subscriber = simple_robot_1dof.forward_kinematics_node_1dof:main',
'forward_kinematics_node_3dof_1 = simple_robot_3dof.forward_kinematics_node_3dof_1:main',
'forward_kinematics_node_3dof_2 = simple_robot_3dof.forward_kinematics_node_3dof_2:main',
'forward_kinematics_node_3dof_3 = simple_robot_3dof.forward_kinematics_node_3dof_3:main',
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 dc6548c..e9e1111 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
@@ -59,7 +59,7 @@ class Forward_Kinematics_Node_3Dof(Node):
def transformation(self):
# Define the desired rotation
- theta = pi / 2
+ theta = - pi / 2
# Split theta
theta_fraction = self.phi * theta
@@ -82,6 +82,7 @@ class Forward_Kinematics_Node_3Dof(Node):
])
self.joint_angles = np.zeros(len(self.Slist[0]))
+ self.joint_angles[0] = np.pi
self.joint_angles[1] = np.pi/2
self.joint_angles[3] = np.pi/2
self.joint_angles[5] = np.pi/2
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 07c3c1d..6d97373 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
@@ -68,7 +68,7 @@ class Forward_Kinematics_Node_3Dof(Node):
def rotation(self):
# define the desired rotation
- theta = pi / 2
+ theta = - pi / 2
# split theta
theta_fraction = self.phi * theta
@@ -125,6 +125,7 @@ class Forward_Kinematics_Node_3Dof(Node):
])
self.joint_angles = np.zeros(len(self.Slist[0]))
+ self.joint_angles[0] = np.pi
self.joint_angles[1] = np.pi/2
self.joint_angles[3] = np.pi/2
self.joint_angles[5] = np.pi/2
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 acff272..26f50df 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
@@ -13,32 +13,32 @@ class Reference_Node_3DoF(Node):
#Create a publisher sending the vector as a message
self.publisher_ = self.create_publisher(Float64, '/dof3/reference_angles', 10)
- self.declare_parameter('velocity', 0.0)
+ self.declare_parameter('max_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.declare_parameter('sequential', False)
+ self.declare_parameter('simultaneous', True)
- self.velocity = self.get_parameter('velocity').get_parameter_value().double_value
+ self.max_velocity = self.get_parameter('max_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
- self.sequential = self.get_parameter('sequential').get_parameter_value().bool_value
+ self.simultaneous = self.get_parameter('simultaneous').get_parameter_value().bool_value
self.phi_start = 0.0
self.phi_end = 1.0
self.phi = self.phi_start
- self.velocity_start = self.velocity
+ self.transition_point = 0.0
self.index = 0
self.timer_period = 0.1 # seconds
self.timer = self.create_timer(self.timer_period, self.ref_point_publisher)
+
+ self.current_velocity = self.timer_period * self.acceleration
# List the points
def ref_point_publisher(self):
@@ -47,39 +47,41 @@ class Reference_Node_3DoF(Node):
msg = Float64()
- if self.sequential == False:
+ if self.simultaneous:
if self.constant_speed == True:
self.phi = self.phi + self.velocity_constant
elif self.constant_speed == False:
- if self.phi < self.slope:
- 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
+ if self.current_velocity < self.max_velocity and self.phi < 0.5:
+ self.phi = self.phi + self.current_velocity
+ self.current_velocity = self.current_velocity + self.timer_period * self.acceleration
+ self.transition_point = self.phi
+ elif self.phi > 1 - self.transition_point:
+ self.phi = self.phi + self.current_velocity
+ self.current_velocity = self.current_velocity - self.timer_period * self.decelaration
else:
- self.phi = self.phi + self.velocity
- elif self.sequential == True:
+ self.phi = self.phi + self.current_velocity
+ else:
if self.constant_speed == True:
- self.phi = self.phi + self.velocity_constant
+ self.phi = self.phi + self.current_velocity
elif self.constant_speed == False:
if self.index % 2 == 0:
self.phi = self.phi + self.velocity_constant
else:
- if self.phi < self.slope:
- 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
+ if self.current_velocity < self.max_velocity:
+ self.phi = self.phi + self.current_velocity
+ self.current_velocity = self.current_velocity + self.timer_period * self.acceleration
+ self.transition_point = self.phi
+ elif self.phi > 1 - self.transition_point:
+ self.phi = self.phi + self.current_velocity
+ self.current_velocity = self.current_velocity - self.timer_period * self.decelaration
else:
- self.phi = self.phi + self.velocity
+ self.phi = self.phi + self.current_velocity
#setting back to 0
if self.phi>self.phi_end:
self.phi = self.phi_start
self.index += 1
- self.velocity = self.velocity_start
+ self.current_velocity = self.timer_period * self.acceleration
# self.get_logger().info('Resetting "%f"' % self.phi)
msg.data = self.phi
--
GitLab