add anchor relay to feedback topics for bio,arm,core

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -84,6 +84,21 @@ class SerialRelay(Node):
             if output:
                 # All output over debug temporarily
                 self.get_logger().info(f"[MCU] {output}")
+                if output.startswith("can_relay_fromvic,arm"):
+                    # Publish the message to the arm topic
+                    msg = String()
+                    msg.data = output
+                    self.arm_pub.publish(msg)
+                elif output.startswith("can_relay_fromvic,core"):
+                    # Publish the message to the core topic
+                    msg = String()
+                    msg.data = output
+                    self.core_pub.publish(msg)
+                elif output.startswith("can_relay_fromvic,bio"):
+                    # Publish the message to the bio topic
+                    msg = String()
+                    msg.data = output
+                    self.bio_pub.publish(msg)
                 msg = String()
                 msg.data = output
                 self.debug_pub.publish(msg)

src/arm_pkg/arm_pkg/arm_node.py

@@ -206,10 +206,10 @@ class SerialRelay(Node):
                 # Extract the voltage from the string
                 voltages_in = parts[3:7]
                 # Convert the voltages to floats
-                self.arm_feedback.bat_voltage = float(voltages_in[0]) / 100.0
-                self.arm_feedback.voltage_12 = float(voltages_in[1]) / 100.0
-                self.arm_feedback.voltage_5 = float(voltages_in[2]) / 100.0
-                self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0
+                # self.arm_feedback.bat_voltage = float(voltages_in[0]) / 100.0
+                # self.arm_feedback.voltage_12 = float(voltages_in[1]) / 100.0
+                # self.arm_feedback.voltage_5 = float(voltages_in[2]) / 100.0
+                # self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0
             else:
                 self.get_logger().info("Invalid voltage feedback input format")
                 

src/arm_pkg/arm_pkg/astra_arm.py

@@ -0,0 +1,137 @@
+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
+from geometry_msgs.msg import Vector3  
+
+
+# Misc
+degree = pi / 180.0
+
+
+def convert_angles(angles):
+    # Converts angles to the format used for the urdf (contains some dummy joints)
+    return [0.0, angles[0]*degree, angles[1]*degree, 0.0, angles[2]*degree, 0.0, angles[3]*degree, 0.0, angles[4]*degree, angles[5]*degree, 0.0]
+
+
+class Arm:
+    def __init__(self, urdf_name):
+        self.ik_tolerance = 1e-3 #Tolerance (in meters) to determine if solution is valid
+        # URDF file path
+        self.urdf = os.path.join(get_package_share_directory('arm_pkg'), urdf_name)
+        # IKpy Chain        
+        self.chain = Chain.from_urdf_file(self.urdf)   
+
+
+        # Arrays for joint states
+        # Some links in the URDF are static (non-joints), these will remain zero for IK
+        # Indexes: Fixed_base, Ax_0, Ax_1, seg1, Ax_2, seg2, ax_3, seg3, continuous, wrist, Effector
+        self.zero_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        self.current_angles = self.zero_angles
+        self.last_angles = self.zero_angles
+        self.ik_angles = self.zero_angles
+
+        self.current_position = []
+        self.target_position = [0.0, 0.0, 0.0] 
+        self.target_orientation = [] # Effector orientation desired at target position. 
+                                # Generally orientation for the effector is modified manually by the operator. 
+        
+        # Might not need, copied over from state_publisher.py in ik_test
+        #self.step = 0.03 # Max movement increment
+
+
+    def perform_ik(self, target_position):
+        self.target_position = target_position
+        # Update the target orientation to the current orientation
+        self.update_orientation()
+        # print(f"[IK FOR] Target Position: {self.target_position}")
+        try:
+            # print(f"[TRY] Current Angles: {self.current_angles}")
+            # print(f"[TRY] Target Position: {self.target_position}")
+            # print(f"[TRY] Target Orientation: {self.target_orientation}")
+            self.ik_angles = self.chain.inverse_kinematics(
+                target_position=self.target_position,
+                target_orientation=self.target_orientation,
+                initial_position=self.current_angles,
+                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]
+            
+            # print(f"[TRY] FK Position for Solution: {fk_position}")
+
+            error = np.linalg.norm(target_position - fk_position)
+            if error > self.ik_tolerance:
+                print(f"No VALID IK Solution within tolerance. Error: {error}")
+                return False
+            else:
+                print(f"IK Solution Found. Error: {error}")
+                return True
+        except Exception as e:
+            print(f"IK failed for exception: {e}")
+            return False
+
+    # Get current orientation of the end effector and update target_orientation
+    def update_orientation(self):
+
+        # FK matrix for arm's current pose
+        fk_matrix = self.chain.forward_kinematics(self.current_angles)
+        
+        # Update target_orientation to the effector's current orientation
+        self.target_orientation = fk_matrix[:3, :3]
+
+    # Update current angles to those provided
+    # Resetting last_angles to the new angles
+    #
+    # Use: First call, or when angles are changed manually.
+    def reset_angles(self, angles):
+        # Update angles to the new angles
+        self.current_angles = convert_angles(angles)
+        self.last_angles = self.current_angles
+
+    # Update current angles to those provided
+    # Maintain previous angles in last_angles
+    #
+    # Use: Repeated calls during IK operation
+    def update_angles(self, angles):
+        # Update angles to the new angles
+        self.last_angles = self.current_angles
+        self.current_angles = convert_angles(angles)
+    
+    # Get current X,Y,Z position of end effector
+    def get_position(self):
+        # FK matrix for arm's current pose
+        fk_matrix = self.chain.forward_kinematics(self.current_angles)
+        
+        # Get the position of the end effector from the FK matrix
+        position = fk_matrix[:3, 3]
+        
+        return position
+
+    # Get current X,Y,Z position of end effector
+    def get_position_vector(self):
+        # FK matrix for arm's current pose
+        fk_matrix = self.chain.forward_kinematics(self.current_angles)
+        
+        # Get the position of the end effector from the FK matrix
+        position = fk_matrix[:3, 3]
+
+        # Return position as a NumPy array
+        return np.array(position)
+
+    
+    def update_position(self):
+        # FK matrix for arm's current pose
+        fk_matrix = self.chain.forward_kinematics(self.current_angles)
+        
+        # Get the position of the end effector from the FK matrix and update current pos
+        self.current_position = fk_matrix[:3, 3]
+
+
+