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 1254fd9fe646d8d5019b1304c66921be0884e800..7553b5e8ac95e6ee41a9770c21827a55e81f6edf 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
@@ -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 = 'True') # 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,
+ decelaration_launch_arg,
slope_launch_arg,
- velocity3_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},
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 281f6a8462db46793101970c4d4bf51ae3933a57..4febcff454c4f630e003b37b5a130566db2fbcb4 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
@@ -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,
+ decelaration_launch_arg,
slope_launch_arg,
- velocity3_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},
diff --git a/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_wheelchair_launch.py b/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_wheelchair_launch.py
deleted file mode 100644
index ec15d44f1b9e11905f89c54abb065ea40ebbda60..0000000000000000000000000000000000000000
--- a/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_wheelchair_launch.py
+++ /dev/null
@@ -1,46 +0,0 @@
-import os
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from launch.actions import ExecuteProcess
-from ament_index_python.packages import get_package_share_directory
-
-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'
- )
-
- return LaunchDescription([
- Node(
- package = package_name,
- executable = 'forward_kinematics_node_3dof_3',
- name = 'forward_kinematics_node',
- output = 'screen',
- parameters = []
- ),
- Node(
- package = package_name,
- executable = 'reference_node_3dof',
- name = 'reference_node',
- output = 'screen',
- parameters = []
- ),
- 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 794a2d33dfd1650ccf33368e9c7c9bfb1aa7d323..25b8c83cabd0fb700a0375adb47cdd3540824c12 100644
--- a/ros2_ws/src/simple_robot_3dof/setup.py
+++ b/ros2_ws/src/simple_robot_3dof/setup.py
@@ -12,7 +12,6 @@ setup(
('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_wheelchair_launch.py']),
('share/' + package_name + '/rviz', ['rviz/forward_kinematics_simulations.rviz']),
],
install_requires=['setuptools'],
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
deleted file mode 100644
index 82d6c5e3a1b7561493cfef1a04741c74c5e06f8a..0000000000000000000000000000000000000000
--- a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_3.py
+++ /dev/null
@@ -1,219 +0,0 @@
-import rclpy
-import os
-from modern_robotics import FKinBody
-from ament_index_python.packages import get_package_share_directory
-from mr_urdf_loader import loadURDF
-from rclpy.node import Node
-from math import sin, cos, pi
-import numpy as np
-from std_msgs.msg import Float64
-from nav_msgs.msg import Path
-from geometry_msgs.msg import PoseStamped
-from geometry_msgs.msg import Pose
-from visualization_msgs.msg import Marker
-import transformations
-from scipy.spatial.transform import Rotation
-import time
-
-
-class Forward_Kinematics_Node_3Dof(Node):
-
- def __init__(self):
- super().__init__('forward_kinematics_node_3dof')
- # Create a subscriber, initialising the transformation
- self.subscription = self.create_subscription(
- Float64,
- '/dof3/reference_angles',
- self.listener_callback,
- 10)
-
-
- urdf_file_path = os.path.join(
- get_package_share_directory('iiwa_description'), 'urdf', 'iiwa14.urdf')
- self.robot_model = loadURDF(urdf_file_path)
- self.M = self.robot_model["M"]
- self.Slist = self.robot_model["Slist"]
- self.Mlist = self.robot_model["Mlist"]
- self.Glist = self.robot_model["Glist"]
- self.Blist = self.robot_model["Blist"]
-
-
- # Create two publishers for the RViz simulation - one for the whole path and one for the marker
- self.path_publisher_ = self.create_publisher(Path, '/simple_robot/visualization_path', 10)
- self.point_publisher_ = self.create_publisher(Marker, '/simple_robot/visualization_marker', 10)
- self.index = 0
-
- self.pose_publisher_simple_robot_ = self.create_publisher(PoseStamped, '/simple_robot/end_effector_pose', 10)
- self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, '/kuka_iiwa/target_pose', 10)
- #RViz
- self.path = Path()
- self.path.header.frame_id = "map"
- self.path.header.stamp = self.get_clock().now().to_msg()
-
-
- def listener_callback(self, msg):
- # Get the message
- self.phi = msg.data
- # Define index and if phi is resetted add 1 to the index
- if(self.phi == 0.0):
- self.index +=1
-
- # check which function to call
- if(self.index % 2 == 0):
- # call the transformation function
- self.translation()
- else:
- self.rotation()
-
-
- def translation(self):
- # define the desired rotation
- theta = pi / 2
- # split theta
- theta_fraction = self.phi * theta
-
- # definte the desired height
- h = 0.3
- # split h
- h_fraction = self.phi * h
-
- # define the matrix
- intermediate_translation = np.array([
- [cos(theta_fraction), -sin(theta_fraction), 0, 0],
- [sin(theta_fraction), cos(theta_fraction), 0, 0],
- [0, 0, 1, h_fraction],
- [0, 0, 0, 1]
- ])
-
- translation_matrix = np.array([
- [cos(theta), -sin(theta), 0, 0],
- [sin(theta), cos(theta), 0, 0],
- [0, 0, 1, h],
- [0, 0, 0, 1]
- ])
-
- self.joint_angles = np.zeros(len(self.Slist[0]))
- self.joint_angles[1] = np.pi/2
- self.joint_angles[3] = np.pi/2
- self.joint_angles[5] = np.pi/2
-
- start_forward_kinematics = FKinBody(self.M, self.Blist, self.joint_angles)
-
- start_position = start_forward_kinematics[:4, 3]
- start_orientation = Rotation.from_matrix(start_forward_kinematics[:3, :3]).as_quat()
-
- self.end_forward_kinematics_1 = np.matmul(intermediate_translation, start_forward_kinematics)
-
- end_position_1 = self.end_forward_kinematics_1[:4, 3]
- end_orientation_1 = Rotation.from_matrix(self.end_forward_kinematics_1[:3, :3]).as_quat()
-
- self.publish_pose_simple_robot(end_position_1, end_orientation_1)
- self.publish_pose_target_pose(end_position_1, end_orientation_1)
- # construct the path of the rotation
- self.construct_path(end_position_1)
-
- def rotation(self):
- # define the desired rotation
- alpha = - pi / 6
- # split alpha
- alpha_fraction = self.phi * alpha
-
- # define the matrix
- intermediate_rotation = np.array([
- [1, 0, 0, 0],
- [0, cos(alpha_fraction), -sin(alpha_fraction), 0],
- [0, sin(alpha_fraction), cos(alpha_fraction), 0],
- [0, 0, 0, 1]
- ])
-
- end_forward_kinematics_2 = np.matmul(intermediate_rotation, self.end_forward_kinematics_1)
-
- 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_simple_robot(end_position_2, end_orientation_2)
- self.publish_pose_target_pose(end_position_2, end_orientation_2)
-
- # construct the path of the rotation
- self.construct_path(end_position_2)
-
-
- def construct_path(self, vector):
- # RViz
- pose = PoseStamped()
- pose.header.frame_id = "map"
- pose.header.stamp = self.get_clock().now().to_msg()
- pose.pose.position.x = vector[0]
- pose.pose.position.y = vector[1]
- pose.pose.position.z = vector[2]
- pose.pose.orientation.w = 1.0
-
- # if the path is completed clear it
- if self.phi == 0 and self.index %2 == 0:
- self.path.poses.clear()
- else:
- self.path.poses.append(pose)
-
- # Publish the path
- self.path_publisher_.publish(self.path)
-
- # Call the function publishing the marker
- self.publish_marker(pose.pose)
-
- def publish_marker(self, current_pose):
- # RViz
- marker = Marker()
- marker.header.frame_id = "map"
- marker.header.stamp = self.get_clock().now().to_msg()
- marker.type = Marker.SPHERE
- marker.action = Marker.ADD
- marker.pose = current_pose
- marker.scale.x = 0.05
- marker.scale.y = 0.05
- marker.scale.z = 0.05
- marker.color.a = 1.0
- marker.color.r = 1.0
- marker.color.g = 0.0
- marker.color.b = 0.0
-
- # Publish the marker
- self.point_publisher_.publish(marker)
-
- def publish_pose_simple_robot(self, vector, quaternion):
- pose_msg = PoseStamped()
- pose_msg.header.frame_id = "map"
- pose_msg.header.stamp = self.get_clock().now().to_msg()
- pose_msg.pose.position.x = vector[0]
- pose_msg.pose.position.y = vector[1]
- pose_msg.pose.position.z = vector[2]
- pose_msg.pose.orientation.x = quaternion[0]
- pose_msg.pose.orientation.y = quaternion[1]
- pose_msg.pose.orientation.z = quaternion[2]
- pose_msg.pose.orientation.w = quaternion[3]
- self.pose_publisher_simple_robot_.publish(pose_msg)
-
- def publish_pose_target_pose(self, vector, quaternion):
- # Create a Pose message for the target pose
- pose_msg = Pose()
- pose_msg.position.x = vector[0]
- pose_msg.position.y = vector[1]
- pose_msg.position.z = vector[2]
- pose_msg.orientation.x = quaternion[0]
- pose_msg.orientation.y = quaternion[1]
- pose_msg.orientation.z = quaternion[2]
- pose_msg.orientation.w = quaternion[3]
- self.pose_publisher_kuka_robot_.publish(pose_msg)
- # if self.phi == 0.0:
- # time.sleep(1)
-
-
-
-def main(args = None):
- rclpy.init(args = args)
- node = Forward_Kinematics_Node_3Dof()
- rclpy.spin(node)
- node.destroy_node()
- rclpy.shutdown()
-
-if __name__ == '__main__':
- main()
\ No newline at end of file
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 3f60cedf3dff0d719f04b350436b8e572ec3de6f..acff272d88dc7508416859a967d196d91c592af6 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,26 +13,27 @@ 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('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.index = 0
@@ -48,32 +49,37 @@ class Reference_Node_3DoF(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.index % 2 == 0:
- self.phi = self.phi + self.delta
+ self.phi = self.phi + self.velocity_constant
else:
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
#setting back to 0
if self.phi>self.phi_end:
self.phi = self.phi_start
self.index += 1
+ self.velocity = self.velocity_start
# self.get_logger().info('Resetting "%f"' % self.phi)
msg.data = self.phi