From 60952db588f6a425a14e356c025a602abf2dc760 Mon Sep 17 00:00:00 2001
From: Tristan McGinnis <tristanmcginnis26@gmail.com>
Date: Sat, 26 Apr 2025 12:17:10 -0500
Subject: [PATCH] swap to numpy arrays

---
 src/arm_pkg/arm_pkg/arm_node.py  | 27 +++++++++------------------
 src/arm_pkg/arm_pkg/astra_arm.py |  9 ++-------
 2 files changed, 11 insertions(+), 25 deletions(-)

diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py
index 29e2379..980015d 100644
--- a/src/arm_pkg/arm_pkg/arm_node.py
+++ b/src/arm_pkg/arm_pkg/arm_node.py
@@ -395,32 +395,25 @@ class SerialRelay(Node):
 
 
     def send_ik(self, msg):
-        input_raw = msg.movement_vector # [x, y, z]
-        #input_x = input_raw[0]
-        #input_y = input_raw[1]
-        #input_z = input_raw[2]
+        input_raw = np.array(msg.movement_vector)  # Convert input to a NumPy array
 
         # Debug output
         tempMsg = String()
         tempMsg.data = "From IK Control Got Vector: " + str(input_raw)
-        #self.get_logger().info(f"[IK Control] {tempMsg.data}")
         self.debug_pub.publish(tempMsg)
 
-        # normalize the vector
-        #input_norm = np.linalg.norm(input_raw) / 2.0
+        # Target position is current position + input vector
+        current_position = self.arm.get_position_vector()
+        target_position = current_position + input_raw
 
-
-
-
-        #Target position is current position + normalized vector
-        #target_position = self.arm.get_position_vector() + input_raw
-        target_position = map(sum, zip(self.arm.get_position_vector(), input_raw))
+        # Debug output for target position
         tempMsg.data = "Target Position: " + str(target_position)
         self.debug_pub.publish(tempMsg)
 
-        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"
+        # Perform IK with the target position
+        if self.arm.perform_ik(target_position):
+            # Send command to control
+            command = "can_relay_tovic,arm,32," + ",".join(map(str, self.arm.ik_angles[:4])) + "\n"
             self.send_cmd(command)
             self.get_logger().info(f"IK Success: {target_position}")
 
@@ -429,14 +422,12 @@ class SerialRelay(Node):
             self.debug_pub.publish(msg)
             msg.data = "Sending: " + str(command)
             self.debug_pub.publish(msg)
-       
         else:
             self.get_logger().info("IK Fail")
             msg = String()
             msg.data = "IK Fail"
             self.debug_pub.publish(msg)
 
-
         # Manual control for Wrist/Effector
         command = "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
         self.send_cmd(command)
diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py
index 7d3cb24..e9d4a84 100644
--- a/src/arm_pkg/arm_pkg/astra_arm.py
+++ b/src/arm_pkg/arm_pkg/astra_arm.py
@@ -114,13 +114,8 @@ class Arm:
         # Get the position of the end effector from the FK matrix
         position = fk_matrix[:3, 3]
 
-        # Convert position to Vector3
-        position_vector = Vector3()
-        position_vector.x = position[0]
-        position_vector.y = position[1]
-        position_vector.z = position[2]
-        
-        return position_vector
+        # Return position as a NumPy array
+        return np.array(position)
 
     
     def update_position(self):