Add urdf folder and arm11.urdf

src/arm_pkg/arm_pkg/arm_node.py

@@ -13,6 +13,18 @@ from ros2_interfaces_pkg.msg import ArmIK
 from ros2_interfaces_pkg.msg import SocketFeedback
 from ros2_interfaces_pkg.msg import DigitFeedback
 
+
+# IK-Related imports
+import numpy as np
+import time, math, os
+from math import sin, cos, pi
+from ament_index_python.packages import get_package_share_directory
+from ikpy.chain import Chain
+from ikpy.link import OriginLink, URDFLink
+#import pygame as pyg
+from scipy.spatial.transform import Rotation as R
+
+
 serial_pub = None
 thread = None
 
@@ -43,8 +55,39 @@ class SerialRelay(Node):
 
         self.arm_feedback = SocketFeedback()
         self.digit_feedback = DigitFeedback()
+        ########
+        # Feedback and IK vars
+        ########
+        # Misc
+        degree = pi / 180.0
 
+        urdf_file_name = 'arm11.urdf'
+        ik_tolerance = 1e-3 #Tolerance (in meters) to determine if solution is valid
+
+        # URDF file path
+        urdf = os.path.join(get_package_share_directory('ik_pkg'), urdf_file_name)
+        # IKpy Chain        
+        arm_chain = Chain.from_urdf_file(urdf)
+        
+        # Arrays for joint states
+        # Some links in the URDF are static (non-joints), these will remain zero for IK
+        # Indexes: Ignore, Ignore, Ax_0, Ax_1, Ax2, Ax_3, Wrist, Ignore
+        zero_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        current_angles = zero_angles
+        last_angles = zero_angles
+        ik_angles = zero_angles
+
+        target_position = [0.0, 0.0, 0.0] 
+        target_orientation = [] # Effector orientation desired at target position. 
+                                # Generally orientation for the effector is modified manually by the operator. 
+
+        
+
+        ########
+        # Interface with MCU 
         # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
+        ########
+
         if self.launch_mode == 'arm':
             # Loop through all serial devices on the computer to check for the MCU
             self.port = None
@@ -123,8 +166,6 @@ class SerialRelay(Node):
                 self.ser.close()
             pass
 
-    def send_ik(self, msg):
-        pass
 
     def send_manual(self, msg: ArmManual):
         axis0 = msg.axis0
@@ -278,6 +319,20 @@ class SerialRelay(Node):
             self.get_logger().info("Invalid voltage feedback input format")
 
 
+    def send_ik(self, msg):
+        pass
+
+    # Given the FK_Matix for the arm's current pose, update the orientation array
+    def update_orientation(self, fk_matrix):
+        self.target_orientation = fk_matrix[:3, :3]
+        return
+    
+    def update_joints(self, ax_0, ax_1, ax_2, ax_3, wrist):
+        self.current_angles = [0.0, 0.0, ax_0, ax_1, ax_2, ax_3, wrist, 0.0]
+        return
+
+
+
     @staticmethod
     def list_serial_ports():
         return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")