feat: IK result calculated and sent to MCU

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

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