From 732cb8c1b59542e388e921443fecbbb6dd395797 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 3 May 2025 18:03:12 -0500 Subject: [PATCH] Revert "add anchor relay to feedback topics for bio,arm,core" This reverts commit d38a84abca96373eab8e87349166e8ebbbcf82a0. --- src/anchor_pkg/anchor_pkg/anchor_node.py | 15 --- src/arm_pkg/arm_pkg/arm_node.py | 8 +- src/arm_pkg/arm_pkg/astra_arm.py | 137 ----------------------- 3 files changed, 4 insertions(+), 156 deletions(-) delete mode 100644 src/arm_pkg/arm_pkg/astra_arm.py diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index e1115f2..859fff0 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -84,21 +84,6 @@ 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) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 3534ca1..c7fad9b 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/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") diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py deleted file mode 100644 index fa92d8d..0000000 --- a/src/arm_pkg/arm_pkg/astra_arm.py +++ /dev/null @@ -1,137 +0,0 @@ -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] - - -