From 37ed60d81335573ac7788d4e7e18c05b171a2112 Mon Sep 17 00:00:00 2001
From: uquzq <ventsi@DESKTOP-J41TDPJ>
Date: Thu, 27 Jun 2024 16:05:55 +0200
Subject: [PATCH] Add quaternions to forward_kinematics_3dof_1/2 nodes
---
.../forward_kinematics_node_3dof_1.py | 28 ++++++++++++++++---
.../forward_kinematics_node_3dof_2.py | 24 ++++++++++++++++
2 files changed, 48 insertions(+), 4 deletions(-)
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 d09b3de..86ccd65 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
@@ -6,6 +6,7 @@ from std_msgs.msg import Float64
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import Marker
+import transformations
class Forward_Kinematics_Node_3Dof(Node):
@@ -20,6 +21,8 @@ class Forward_Kinematics_Node_3Dof(Node):
# Create two publishers for the RViz simulation - one for the whole path and one for the marker
self.path_publisher_ = self.create_publisher(Path, 'visualization_path', 10)
self.point_publisher_ = self.create_publisher(Marker, 'visualization_marker', 10)
+
+ self.pose_publisher_ = self.create_publisher(PoseStamped, '/end_effector_pose_3dof_1', 10)
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -33,7 +36,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
@@ -48,12 +51,12 @@ class Forward_Kinematics_Node_3Dof(Node):
d_fraction = self.phi * d
# Define the vector
- strart_vector = np.array([-1.0, 0.0, 0.0, 1.0])
+ strart_vector = np.array([1.0, 0.0, 0.0, 1.0])
# Define the matrix
intermediate_translation = np.array([
- [cos(theta_fraction), -sin(theta_fraction), 0, d_fraction],
- [sin(theta_fraction), cos(theta_fraction), 0, 0],
+ [cos(theta_fraction), -sin(theta_fraction), 0, 0],
+ [sin(theta_fraction), cos(theta_fraction), 0, d_fraction],
[0, 0, 1, h_fraction],
[0, 0, 0, 1]
])
@@ -61,6 +64,10 @@ class Forward_Kinematics_Node_3Dof(Node):
# Calculate the intermediate vectors
intermediate_vector = np.matmul(intermediate_translation, strart_vector)
+ quaternion_1 = transformations.quaternion_from_euler(0.0, 0.0, theta_fraction)
+
+ self.publish_pose(intermediate_vector, quaternion_1)
+
self.construct_path(intermediate_vector)
def construct_path(self, vector):
@@ -104,6 +111,19 @@ class Forward_Kinematics_Node_3Dof(Node):
# Publish the marker
self.point_publisher_.publish(marker)
+ def publish_pose(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_.publish(pose_msg)
+
def main(args = None):
rclpy.init(args = args)
node = Forward_Kinematics_Node_3Dof()
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 49f0302..9e8f7b6 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
@@ -6,6 +6,7 @@ from std_msgs.msg import Float64
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import Marker
+import transformations
class Forward_Kinematics_Node_3Dof(Node):
@@ -25,6 +26,8 @@ class Forward_Kinematics_Node_3Dof(Node):
self.angle_publisher_ = self.create_publisher(Float64, '/angle_3dof/plotjuggler',10)
self.height_publisher_ = self.create_publisher(Float64, '/height_3dof/plotjuggler', 10)
self.length_publisher_ = self.create_publisher(Float64, '/length_3dof/plotjuggler', 10)
+
+ self.pose_publisher_ = self.create_publisher(PoseStamped, '/end_effector_pose_3dof_2', 10)
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -65,6 +68,10 @@ class Forward_Kinematics_Node_3Dof(Node):
# 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)
+
+ self.publish_pose(rotated_intermediate_vector, quaternion_1)
+
# construct the path of the rotation
self.construct_path(rotated_intermediate_vector)
@@ -107,6 +114,10 @@ class Forward_Kinematics_Node_3Dof(Node):
# 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.publish_pose(translated_vector, quaternion_2)
+
# construct the path of the translation
self.construct_path(translated_vector)
@@ -167,6 +178,19 @@ class Forward_Kinematics_Node_3Dof(Node):
length_msg.data = length
self.length_publisher_.publish(length_msg)
+ def publish_pose(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_.publish(pose_msg)
+
def main(args = None):
rclpy.init(args = args)
--
GitLab