Update forward_kinematics_node for the simple_robot simulation

ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py

@@ -35,6 +35,16 @@ class Forward_Kinematics_Node_1Dof(Node):
         self.Glist = self.robot_model["Glist"]
         self.Blist = self.robot_model["Blist"]
 
+        urdf_file_path_simple_robot = os.path.join(
+            get_package_share_directory('simple_robot_1dof'), 'urdf', 'simple_robot.urdf')
+        self.simple_robot_model = loadURDF(urdf_file_path_simple_robot)
+        self.simple_robot_M = self.simple_robot_model["M"]
+        self.simple_robot_Slist = self.simple_robot_model["Slist"]
+        self.simple_robot_Mlist = self.simple_robot_model["Mlist"]
+        self.simple_robot_Glist = self.simple_robot_model["Glist"]
+        self.simple_robot_Blist = self.simple_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)
@@ -57,9 +67,10 @@ class Forward_Kinematics_Node_1Dof(Node):
         if self.phi == 0:
             self.index += 1
 
-        self.transformation()
+        self.transformation_kuka()
+        self.transformation_simple_robot()
         
-    def transformation(self):
+    def transformation_kuka(self):
         # Define the desired rotation here
         theta = - pi / 1.8
         # Split theta
@@ -92,13 +103,43 @@ class Forward_Kinematics_Node_1Dof(Node):
         end_position = end_forward_kinematics[:4, 3]
         end_orientation = Rotation.from_matrix(end_forward_kinematics[:3, :3]).as_quat()
 
-
-        self.publish_pose_simple_robot(end_position, end_orientation)
-
         if self.index == 2:
             self.publish_pose_target_pose(end_position, end_orientation)
+
+
+    def transformation_simple_robot(self):
+        # Define the desired rotation here
+        theta = - pi / 1.8
+        # Split theta
+        theta_fraction = self.phi * theta
+
+        self.publish_angle(theta_fraction)
+
+        # Define the matrix
+        intermediate_rotation = np.array([
+            [cos(theta_fraction), -sin(theta_fraction), 0, 0],
+            [sin(theta_fraction), cos(theta_fraction), 0, 0],
+            [0, 0, 1, 0],
+            [0, 0, 0, 1]
+        ])
+
+        self.simple_robot_joint_angles = np.zeros(len(self.simple_robot_Slist[0]))
+        # self.simple_robot_joint_angles[1] = np.pi / 2
+
+        start_forward_kinematics_simple = FKinBody(self.simple_robot_M, self.simple_robot_Blist, self.simple_robot_joint_angles)
+
+        start_position_simple = start_forward_kinematics_simple[:4 ,3]
+        start_orientation = Rotation.from_matrix(start_forward_kinematics_simple[:3, :3]).as_quat()
+
+        end_forward_kinematics_simple = np.matmul(intermediate_rotation, start_forward_kinematics_simple)
+
+        end_position_simple = end_forward_kinematics_simple[:4, 3]
+        end_orientation_simple = Rotation.from_matrix(end_forward_kinematics_simple[:3, :3]).as_quat()
+
+
+        self.publish_pose_simple_robot(end_position_simple, end_orientation_simple)
         
-        self.construct_path(end_position)
+        self.construct_path(end_position_simple)
 
     def construct_path(self, vector):
         # RViz

ros2_ws/src/simple_robot_1dof/urdf/simple_robot.urdf

@@ -78,6 +78,14 @@
     <limit effort = "300" lower="-2.96705972839" upper="2.96705972839" velocity = "10" />
   </joint>
 
+  <link name="ee_link" />
+
+  <joint name="ee_joint" type="fixed">
+    <parent link="arm3"/>
+    <child link="ee_link"/>
+    <origin xyz="-0.3 0 0"/>
+  </joint>
+
 
 </robot>