diff --git a/ros2_ws/src/iiwa/iiwa_bringup/launch/__pycache__/iiwa.launch.cpython-310.pyc b/ros2_ws/src/iiwa/iiwa_bringup/launch/__pycache__/iiwa.launch.cpython-310.pyc
index 2da6ee64e3d0230ee67e8f5e19f8e4191221ec33..d9aa60a4db91d1a8afe00067ceb231f94e1a72e7 100644
Binary files a/ros2_ws/src/iiwa/iiwa_bringup/launch/__pycache__/iiwa.launch.cpython-310.pyc and b/ros2_ws/src/iiwa/iiwa_bringup/launch/__pycache__/iiwa.launch.cpython-310.pyc differ
diff --git a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py
index 6fa9d3ee556358da5f2fe4c5a79d38384f6ae3ac..a62a3e1abb4746b6aba7b71bd93323ee6587348d 100644
--- a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py
+++ b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py
@@ -47,8 +47,8 @@ class Reference_Node_1DoF(Node):
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_punkt = self.phi
- elif self.phi > 1 - self.transition_punkt:
+ 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:
diff --git a/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_launch.py b/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_launch.py
new file mode 100644
index 0000000000000000000000000000000000000000..a88263688befc5d33805652c8c4b3cee3bc01bf1
--- /dev/null
+++ b/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_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_2dof'
+
+ 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_2dof_1',
+ name = 'forward_kinematics_node',
+ output = 'screen',
+ parameters = [],
+ condition = IfCondition(simultaneous)
+ )
+ forward_kinematics_node_2 = Node(
+ package = package_name,
+ executable = 'forward_kinematics_node_2dof_2',
+ name = 'forward_kinematics_node',
+ output = 'screen',
+ parameters = [],
+ condition = UnlessCondition(simultaneous)
+ )
+ reference_node = Node(
+ package = package_name,
+ executable = 'reference_node_2dof',
+ name = 'reference_node',
+ output = 'screen',
+ parameters = [
+ {'max_velocity': max_velocity},
+ {'acceleration': acceleration},
+ {'decelaration': decelaration},
+ {'velocity_constant': velocity_constant},
+ {'simultaneous': simultaneous},
+ {'constant_speed': constant_speed}
+ ]
+ )
+ rviz_node = Node(
+ package='rviz2',
+ executable='rviz2',
+ name='rviz2',
+ output='screen',
+ arguments=['-d', rviz_config_file]
+ )
+ inverse_kinematic_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_kinematic_node
+ ]
+
+ return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
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
deleted file mode 100644
index 79769735d432efd1c06ee951856d43dbb91d8f68..0000000000000000000000000000000000000000
--- a/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_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_2dof'
-
- 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,
- slope_launch_arg,
- decelaration_launch_arg,
- velocity_constant_launch_arg,
- constant_speed_launch_arg,
- sequential_launch_arg,
- Node(
- package = package_name,
- executable = 'forward_kinematics_node_2dof_2',
- name = 'forward_kinematics_node',
- output = 'screen',
- parameters = []
- ),
- Node(
- package = package_name,
- executable = 'reference_node_2dof',
- 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_2dof/launch/kuka_rviz_simulation_2dof_simultaneous_launch.py b/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_simultaneous_launch.py
deleted file mode 100644
index 15ecf551eeb87c3f9b10271f1e5286f6d11db644..0000000000000000000000000000000000000000
--- a/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_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_2dof'
-
- 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,
- slope_launch_arg,
- decelaration_launch_arg,
- velocity_constant_launch_arg,
- constant_speed_launch_arg,
- sequential_launch_arg,
- Node(
- package = package_name,
- executable = 'forward_kinematics_node_2dof_1',
- name = 'forward_kinematics_node',
- output = 'screen',
- parameters = []
- ),
- Node(
- package = package_name,
- executable = 'reference_node_2dof',
- 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_2dof/setup.py b/ros2_ws/src/simple_robot_2dof/setup.py
index c5c817cdeb78ef78139bf6ba139ea721bd840d90..656489b314929d2103cf3819485c9da96736d7be 100644
--- a/ros2_ws/src/simple_robot_2dof/setup.py
+++ b/ros2_ws/src/simple_robot_2dof/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_2dof_simultaneous_launch.py']),
- ('share/' + package_name + '/launch', ['launch/kuka_rviz_simulation_2dof_sequential_launch.py']),
+ ('share/' + package_name + '/launch', ['launch/kuka_rviz_simulation_2dof_launch.py']),
('share/' + package_name + '/rviz', ['rviz/forward_kinematics_simulations.rviz']),
],
install_requires=['setuptools'],
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 7be6ee84c6e31a1bcbc5207edd04cad63aaec75e..5f265da5894bb6430f938f8dc3a69b13d25552f4 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
@@ -64,7 +64,7 @@ class Forward_Kinematics_Node_2Dof(Node):
def transformation(self):
# Define the desired rotation
- theta = pi / 1.8
+ theta = - pi / 1.8
# Split theta
theta_fraction = self.phi * theta
@@ -86,6 +86,7 @@ class Forward_Kinematics_Node_2Dof(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_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 a605044e4832c5f88ac22cf247a67dcc36a99a39..047442a5cc6fe399604e35b83f48810d3f6fb6ae 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
@@ -66,7 +66,7 @@ class Forward_Kinematics_Node_2Dof(Node):
def rotation(self):
# define the desired rotation
- theta = pi / 2
+ theta = - pi / 2
# split theta
theta_fraction = self.phi * theta
@@ -85,6 +85,7 @@ class Forward_Kinematics_Node_2Dof(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_2dof/simple_robot_2dof/reference_node_2dof.py b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py
index e71a96ea251e0986a67d197f4b8f6d49072d801d..843662b0e69692565416a2c03c955a27fce6be97 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,30 +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('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.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.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.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):
@@ -45,35 +42,23 @@ class Reference_Node_2DoF(Node):
msg = Float64()
- if self.sequential == False:
- if self.constant_speed == True:
+ 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
- else:
- self.phi = self.phi + self.velocity
- elif self.sequential == True:
- 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
- else:
- self.phi = self.phi + self.velocity
+ elif self.constant_speed == False:
+ 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.current_velocity
#setting back to 0
if self.phi>self.phi_end:
self.phi = self.phi_start
- 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