diff --git a/ros2_ws/src/simple_robot_1dof/launch/forward_kinematics_1dof_launch.py b/ros2_ws/src/simple_robot_1dof/launch/forward_kinematics_1dof_launch.py
index 968c84c8b3bfc163fed7eb0ceefe871aef5540dd..a7343c2abb72c3c83dcee760c427ece180cd383c 100644
--- a/ros2_ws/src/simple_robot_1dof/launch/forward_kinematics_1dof_launch.py
+++ b/ros2_ws/src/simple_robot_1dof/launch/forward_kinematics_1dof_launch.py
@@ -42,4 +42,11 @@ def generate_launch_description():
# name='velocity_param_node',
# output='screen'
# )
+ 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/forward_kinematics_2dof_sequential_launch.py b/ros2_ws/src/simple_robot_2dof/launch/forward_kinematics_2dof_sequential_launch.py
index 5e456fb33d586e7df31bbd19c9e145a95ab46fd6..6a39c00706e54352e13cdc5ee51c7fe648630183 100644
--- a/ros2_ws/src/simple_robot_2dof/launch/forward_kinematics_2dof_sequential_launch.py
+++ b/ros2_ws/src/simple_robot_2dof/launch/forward_kinematics_2dof_sequential_launch.py
@@ -35,5 +35,12 @@ def generate_launch_description():
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/forward_kinematics_2dof_simultaneous_launch.py b/ros2_ws/src/simple_robot_2dof/launch/forward_kinematics_2dof_simultaneous_launch.py
index 06acc978369d64bc848de4fb02b6886c0694c893..39badf8aadaf6cc489c28d3c3f0db7a0bf96764e 100644
--- a/ros2_ws/src/simple_robot_2dof/launch/forward_kinematics_2dof_simultaneous_launch.py
+++ b/ros2_ws/src/simple_robot_2dof/launch/forward_kinematics_2dof_simultaneous_launch.py
@@ -35,5 +35,12 @@ def generate_launch_description():
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/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 a8ba2b304f663a3c1b56b97f35a2ed7e297090e2..802e56a2cfdbf5b7af6e539443129d7f8020399c 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
@@ -1,12 +1,19 @@
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_2Dof(Node):
@@ -19,6 +26,17 @@ class Forward_Kinematics_Node_2Dof(Node):
'/dof2/reference_angles',
self.listener_callback,
10)
+
+ urdf_file_path = os.path.join(
+ get_package_share_directory('iiwa_description'), 'urdf', 'iiwa14.urdf')
+ # self.get_logger().info(f'Loading URDF from: {urdf_file_path}')
+ 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)
@@ -27,7 +45,7 @@ class Forward_Kinematics_Node_2Dof(Node):
self.height_publisher_ = self.create_publisher(Float64, '/simple_robot/height_2dof/plotjuggler', 10)
# create a publisher for the pose
self.pose_publisher_simple_robot_ = self.create_publisher(PoseStamped, '/simple_robot/end_effector_pose', 10)
- self.pose_publisher_kuka_robot_ = self.create_publisher(PoseStamped, '/kuka_robot/end_effector_pose', 10)
+ self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, 'target_pose', 10)
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -41,21 +59,21 @@ class Forward_Kinematics_Node_2Dof(Node):
def transformation(self):
# Define the desired rotation
- theta = pi / 2
+ theta = pi / 1.8
# Split theta
theta_fraction = self.phi * theta
self.publish_angle(theta_fraction)
# Definte the desired height
- h = 1
+ h = 0.3
#Split h
h_fraction = self.phi * h
self.publish_height(h_fraction)
# Define the vector
- start_vector = np.array([1.0, 0.0, 0.0, 1.0])
+ # start_vector = np.array([1.0, 0.0, 0.0, 1.0])
# Define the matrix
intermediate_rotation = np.array([
@@ -64,15 +82,25 @@ class Forward_Kinematics_Node_2Dof(Node):
[0, 0, 1, h_fraction],
[0, 0, 0, 1]
])
-
- # Calculate the intermediate vectors
- intermediate_vector = np.matmul(intermediate_rotation, start_vector)
- quaternion = transformations.quaternion_from_euler(0.0, 0.0, theta_fraction)
+ 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)
- self.publish_pose(intermediate_vector, quaternion)
+ start_position = start_forward_kinematics[:4, 3]
+ start_orientation = Rotation.from_matrix(start_forward_kinematics[:3, :3]).as_quat()
- self.construct_path(intermediate_vector)
+ end_forward_kinematics = np.matmul(intermediate_rotation, start_forward_kinematics)
+
+ end_position = end_forward_kinematics[:4, 3]
+ end_orientation = Rotation.from_matrix(end_forward_kinematics[:3, :3]).as_quat()
+
+ self.publish_pose_target_pose(end_position, end_orientation)
+
+ self.construct_path(end_position)
def construct_path(self, vector):
# RViz
@@ -126,7 +154,7 @@ class Forward_Kinematics_Node_2Dof(Node):
height_msg.data = height
self.height_publisher_.publish(height_msg)
- def publish_pose(self, vector, quaternion):
+ 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()
@@ -138,7 +166,20 @@ class Forward_Kinematics_Node_2Dof(Node):
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):
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 79e3478ebc9b77e159b180bec22dad7ff1f48ee7..245a972201839cb6f8f35a5aa8dd766e90994dce 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
@@ -1,12 +1,19 @@
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_2Dof(Node):
@@ -19,13 +26,24 @@ class Forward_Kinematics_Node_2Dof(Node):
'/dof2/reference_angles',
self.listener_callback,
10)
+
+ urdf_file_path = os.path.join(
+ get_package_share_directory('iiwa_description'), 'urdf', 'iiwa14.urdf')
+ # self.get_logger().info(f'Loading URDF from: {urdf_file_path}')
+ 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(PoseStamped, '/kuka_robot/end_effector_pose', 10)
+ self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, 'target_pose', 10)
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -54,7 +72,7 @@ class Forward_Kinematics_Node_2Dof(Node):
theta_fraction = self.phi * theta
# Define the vector
- start_vector = np.array([1.0, 0.0, 0.0, 1.0])
+ # start_vector = np.array([1.0, 0.0, 0.0, 1.0])
# define the matrix
intermediate_rotation = np.array([
@@ -69,23 +87,38 @@ class Forward_Kinematics_Node_2Dof(Node):
[0, 0, 1, 0],
[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_rotation, 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()
# calculate the intermediate vectors
- rotated_intermediate_vector = np.matmul(intermediate_rotation, start_vector)
+ # rotated_intermediate_vector = np.matmul(intermediate_rotation, start_vector)
- quaternion_1 = transformations.quaternion_from_euler(0.0, 0.0, theta_fraction)
+ # quaternion_1 = transformations.quaternion_from_euler(0.0, 0.0, theta_fraction)
# calculate the end vector
- self.end_vector = np.matmul(rotation, start_vector)
+ self.end_vector = np.matmul(rotation, start_position)
- self.publish_pose(rotated_intermediate_vector, quaternion_1)
+ self.publish_pose_target_pose(end_position_1, end_orientation_1)
# construct the path of the rotation
- self.construct_path(rotated_intermediate_vector)
+ self.construct_path(end_position_1)
def translation(self):
# define the desired height
- h = 1
+ h = 0.3
# split h
h_fraction = self.phi * h
@@ -97,12 +130,17 @@ class Forward_Kinematics_Node_2Dof(Node):
[0, 0, 0, 1]
])
+ end_forward_kinematics_2 = np.matmul(intermediate_translation, 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()
+
# calculate the intermediate vectors
translated_vector = np.matmul(intermediate_translation, self.end_vector)
- quaternion_2 = transformations.quaternion_from_euler(0.0, 0.0, - pi /2)
+ # quaternion_2 = transformations.quaternion_from_euler(0.0, 0.0, - pi /2)
- self.publish_pose(translated_vector, quaternion_2)
+ self.publish_pose_target_pose(end_position_2, end_orientation_2)
# construct the path of the translation
self.construct_path(translated_vector)
@@ -149,7 +187,7 @@ class Forward_Kinematics_Node_2Dof(Node):
self.point_publisher_.publish(marker)
- def publish_pose(self, vector, quaternion):
+ 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()
@@ -161,7 +199,20 @@ class Forward_Kinematics_Node_2Dof(Node):
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):