From f4a611567e69b21942b30d4d59a37de893b05ba9 Mon Sep 17 00:00:00 2001
From: Tristan McGinnis <tristanmcginnis26@gmail.com>
Date: Sat, 12 Apr 2025 14:35:09 -0500
Subject: [PATCH] feat: IK result calculated and sent to MCU

---
 src/arm_pkg/arm_pkg/arm_node.py  | 42 +++++++++++++++++++++++++++++++-
 src/arm_pkg/arm_pkg/astra_arm.py | 13 ++++++++--
 2 files changed, 52 insertions(+), 3 deletions(-)

diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py
index b55498d..736ce5a 100644
--- a/src/arm_pkg/arm_pkg/arm_node.py
+++ b/src/arm_pkg/arm_pkg/arm_node.py
@@ -390,9 +390,49 @@ class SerialRelay(Node):
         msg.joint_voltages = self.arm_feedback.joint_voltages
         msg.joint_currents = self.arm_feedback.joint_currents
 
-        self.socket_pub.publish(msg)
+        self.socket_pub.publish(msg) #Publish feedback from arm
+
+        self.arm.update_position() #Run FK and update the current position of the arm, using FK
+
+
 
     def send_ik(self, msg):
+        input_raw = msg.movement_vector # [x, y, z]
+
+        # normalize the vector
+        input_norm = np.linalg.norm(input_raw) / 2.0
+
+        #Target position is current position + normalized vector
+        target_position = self.arm.get_position() + input_norm
+
+        if(self.arm.perform_ik(self.arm.get_position()+input_norm)):
+            #send command to control
+            command = "can_relay_tovic,arm,32," + str(self.arm.ik_angles[0]) + "," + str(self.arm.ik_angles[1]) + "," + str(self.arm.ik_angles[2]) + "," + str(self.arm.ik_angles[3]) + "\n"
+            self.send_cmd(command)
+            self.get_logger().info(f"IK Success: {target_position}")
+        else:
+            self.get_logger().info("IK Fail")
+
+
+        # Manual control for Wrist/Effector
+        command = "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
+        self.send_cmd(command)
+        
+        command = "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
+        self.send_cmd(command)
+
+        command = "can_relay_tovic,digit,26," + str(msg.gripper) + "\n"
+        self.send_cmd(command)
+
+        command = "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
+        self.send_cmd(command)
+
+        # Placeholder need control for linear actuator
+        #command = ""
+        #self.send_cmd()
+
+
+
         pass
 
     # Given the FK_Matix for the arm's current pose, update the orientation array
diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py
index 2f8002b..80bbe61 100644
--- a/src/arm_pkg/arm_pkg/astra_arm.py
+++ b/src/arm_pkg/arm_pkg/astra_arm.py
@@ -52,9 +52,18 @@ class Arm:
                 target_position=self.target_position,
                 target_orientation=self.target_orientation,
                 initial_position=self.current_angles,
-                max_iterations=1000,
-                tolerance=self.ik_tolerance
+                orientation_mode="all"
             )
+            # Check if the solution is within the tolerance
+            fk_matrix = self.chain.forward_kinematics(self.ik_angles)
+            fk_position = fk_matrix[:3, 3]
+            error = np.linalg.norm(target_position - fk_position)
+            if error > self.ik_tolerance:
+                self.get_logger().info(f"No VALID IK Solution within tolerance. Error: {error}")
+                return False
+            else:
+                self.get_logger().info(f"IK Solution Found. Error: {error}")
+                return True
         except Exception as e:
             print(f"IK failed: {e}")
             return False