Add quaternions to forward_kinematics_3dof_1/2 nodes

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()

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)