diff --git a/ros2_ws/src/simple_robot_3dof/launch/forward_kinematics_3dof_sequential_launch.py b/ros2_ws/src/simple_robot_3dof/launch/forward_kinematics_3dof_sequential_launch.py
index cbeabe9b2c31b3e4aea5a47b7ca10cdf9147ad8a..23376438cba6f76bc9a6c2eb1bb188c945c750ea 100644
--- a/ros2_ws/src/simple_robot_3dof/launch/forward_kinematics_3dof_sequential_launch.py
+++ b/ros2_ws/src/simple_robot_3dof/launch/forward_kinematics_3dof_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_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 996467fa047b61c8edf84646f06c554fda38c867..5d397826ca87b944d017dcc394490caee2ea690b 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
@@ -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_3Dof(Node):
@@ -19,6 +26,16 @@ class Forward_Kinematics_Node_3Dof(Node):
'/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)
@@ -28,7 +45,7 @@ class Forward_Kinematics_Node_3Dof(Node):
self.length_publisher_ = self.create_publisher(Float64, '/simple_robot/length_3dof/plotjuggler', 10)
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"
@@ -65,35 +82,32 @@ class Forward_Kinematics_Node_3Dof(Node):
[0, 0, 1, 0],
[0, 0, 0, 1]
])
-
- # calculate the intermediate vectors
- rotated_intermediate_vector = np.matmul(intermediate_rotation, self.end_vector)
- quaternion_1 = transformations.quaternion_from_euler(0.0, 0.0, theta_fraction)
+ end_forward_kinematics_2 = np.matmul(intermediate_rotation, self.end_forward_kinematics_1)
- self.publish_pose(rotated_intermediate_vector, quaternion_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_target_pose(end_position_2, end_orientation_2)
# construct the path of the rotation
- self.construct_path(rotated_intermediate_vector)
+ self.construct_path(end_position_2)
def translation(self):
# definte the desired height
- h = 0.6
+ h = 0.2
# split h
h_fraction = self.phi * h
self.publish_height(h_fraction)
# define the desired length
- d = 0.35
+ d = 0.22
# split d
d_fraction = self.phi * d
self.publish_length(d_fraction)
- # Define the vector
- start_vector = np.array([1.0, 0.0, 0.0, 1.0])
-
# define translation matrix
intermediate_translation = np.array([
[1, 0, 0, 0],
@@ -109,15 +123,28 @@ class Forward_Kinematics_Node_3Dof(Node):
[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()
+
# calculate the intermediate vectors
- translated_vector = np.matmul(intermediate_translation, start_vector)
+ translated_vector = np.matmul(intermediate_translation, start_position)
# calculate the end vector
- self.end_vector = np.matmul(translation_matrix, start_vector)
-
- quaternion_2 = transformations.quaternion_from_euler(0.0, 0.0, - pi / 2)
+ self.end_vector = np.matmul(translation_matrix, start_position)
- self.publish_pose(translated_vector, quaternion_2)
+ self.publish_pose_target_pose(translated_vector, end_orientation_1)
# construct the path of the translation
self.construct_path(translated_vector)
@@ -167,7 +194,6 @@ class Forward_Kinematics_Node_3Dof(Node):
angle_msg = Float64()
angle_msg.data = angle * 180/ pi
self.angle_publisher_.publish(angle_msg)
- # self.get_logger().info('Publishing: %f' %angle_radians)
def publish_height(self, height):
height_msg = Float64()
@@ -179,7 +205,7 @@ class Forward_Kinematics_Node_3Dof(Node):
length_msg.data = length
self.length_publisher_.publish(length_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()
@@ -191,7 +217,20 @@ class Forward_Kinematics_Node_3Dof(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):