Add quaternions to forward_kinematics_2dof node

ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py

@@ -6,6 +6,8 @@ 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_2Dof(Node):
 
@@ -65,7 +67,9 @@ class Forward_Kinematics_Node_2Dof(Node):
         # Calculate the intermediate vectors
         intermediate_vector = np.matmul(intermediate_rotation, start_vector)
 
-        self.publish_pose(intermediate_vector)
+        quaternion = transformations.quaternion_from_euler(0.0, 0.0, theta_fraction)
+
+        self.publish_pose(intermediate_vector, quaternion)
 
         self.construct_path(intermediate_vector)
 
@@ -121,17 +125,17 @@ class Forward_Kinematics_Node_2Dof(Node):
         height_msg.data = height
         self.height_publisher_.publish(height_msg)
 
-    def publish_pose(self, vector):
+    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 = 0.0
-        pose_msg.pose.orientation.y = 0.0
-        pose_msg.pose.orientation.z = 0.0
-        pose_msg.pose.orientation.w = 1.0
+        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)
 
 

ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_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_2Dof(Node):
@@ -22,6 +23,8 @@ class Forward_Kinematics_Node_2Dof(Node):
         self.path_publisher_ = self.create_publisher(Path, 'visualization_path', 10)
         self.point_publisher_ = self.create_publisher(Marker, 'visualization_marker', 10)
         self.index = 0
+
+        self.pose_publisher_ = self.create_publisher(PoseStamped, '/end_effector_pose_2dof_1', 10)
         #RViz
         self.path = Path()
         self.path.header.frame_id = "map"
@@ -45,12 +48,12 @@ class Forward_Kinematics_Node_2Dof(Node):
         
     def rotation(self):
         # define the desired rotation 
-        theta = -pi / 2
+        theta = pi / 2
         # split theta
         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,9 +72,13 @@ class Forward_Kinematics_Node_2Dof(Node):
         # calculate the intermediate vectors
         rotated_intermediate_vector = np.matmul(intermediate_rotation, start_vector)
 
+        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.publish_pose(rotated_intermediate_vector, quaternion_1)
+
         # construct the path of the rotation
         self.construct_path(rotated_intermediate_vector)
 
@@ -92,6 +99,10 @@ class Forward_Kinematics_Node_2Dof(Node):
         # 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)
+
+        self.publish_pose(translated_vector, quaternion_2)
+
         # construct the path of the translation
         self.construct_path(translated_vector)
 
@@ -137,6 +148,20 @@ class Forward_Kinematics_Node_2Dof(Node):
         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_2Dof()