From 0adab485f2add6482b99be81ea50a7c05c871044 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 22 Mar 2025 11:17:18 -0500 Subject: [PATCH 01/49] Add urdf folder and arm11.urdf --- src/arm_pkg/arm_pkg/arm_node.py | 59 +++++++- src/arm_pkg/setup.py | 4 + src/arm_pkg/urdf/arm11.urdf | 239 ++++++++++++++++++++++++++++++++ 3 files changed, 300 insertions(+), 2 deletions(-) create mode 100644 src/arm_pkg/urdf/arm11.urdf diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index e7549d6..e008020 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -13,6 +13,18 @@ from ros2_interfaces_pkg.msg import ArmIK from ros2_interfaces_pkg.msg import SocketFeedback from ros2_interfaces_pkg.msg import DigitFeedback + +# IK-Related imports +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 + + serial_pub = None thread = None @@ -43,8 +55,39 @@ class SerialRelay(Node): self.arm_feedback = SocketFeedback() self.digit_feedback = DigitFeedback() + ######## + # Feedback and IK vars + ######## + # Misc + degree = pi / 180.0 + urdf_file_name = 'arm11.urdf' + ik_tolerance = 1e-3 #Tolerance (in meters) to determine if solution is valid + + # URDF file path + urdf = os.path.join(get_package_share_directory('ik_pkg'), urdf_file_name) + # IKpy Chain + arm_chain = Chain.from_urdf_file(urdf) + + # Arrays for joint states + # Some links in the URDF are static (non-joints), these will remain zero for IK + # Indexes: Ignore, Ignore, Ax_0, Ax_1, Ax2, Ax_3, Wrist, Ignore + zero_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + current_angles = zero_angles + last_angles = zero_angles + ik_angles = zero_angles + + target_position = [0.0, 0.0, 0.0] + target_orientation = [] # Effector orientation desired at target position. + # Generally orientation for the effector is modified manually by the operator. + + + + ######## + # Interface with MCU # Search for ports IF in 'arm' (standalone) and not 'anchor' mode + ######## + if self.launch_mode == 'arm': # Loop through all serial devices on the computer to check for the MCU self.port = None @@ -123,8 +166,6 @@ class SerialRelay(Node): self.ser.close() pass - def send_ik(self, msg): - pass def send_manual(self, msg: ArmManual): axis0 = msg.axis0 @@ -278,6 +319,20 @@ class SerialRelay(Node): self.get_logger().info("Invalid voltage feedback input format") + def send_ik(self, msg): + pass + + # Given the FK_Matix for the arm's current pose, update the orientation array + def update_orientation(self, fk_matrix): + self.target_orientation = fk_matrix[:3, :3] + return + + def update_joints(self, ax_0, ax_1, ax_2, ax_3, wrist): + self.current_angles = [0.0, 0.0, ax_0, ax_1, ax_2, ax_3, wrist, 0.0] + return + + + @staticmethod def list_serial_ports(): return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*") diff --git a/src/arm_pkg/setup.py b/src/arm_pkg/setup.py index 8c8c3bd..6d55985 100644 --- a/src/arm_pkg/setup.py +++ b/src/arm_pkg/setup.py @@ -1,4 +1,6 @@ from setuptools import find_packages, setup +import os +from glob import glob package_name = 'arm_pkg' @@ -10,6 +12,8 @@ setup( ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + (os.path.join('share', package_name), glob('urdf/*')), ], install_requires=['setuptools'], zip_safe=True, diff --git a/src/arm_pkg/urdf/arm11.urdf b/src/arm_pkg/urdf/arm11.urdf new file mode 100644 index 0000000..2fb7036 --- /dev/null +++ b/src/arm_pkg/urdf/arm11.urdf @@ -0,0 +1,239 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From ec3b95944dd458082ac9dd5cfed46a6fded175b5 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Fri, 4 Apr 2025 19:20:13 -0500 Subject: [PATCH 02/49] Add helper functions to arm class --- src/arm_pkg/arm_pkg/astra_arm.py | 92 ++++++++++++++++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 src/arm_pkg/arm_pkg/astra_arm.py diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py new file mode 100644 index 0000000..9bc84fd --- /dev/null +++ b/src/arm_pkg/arm_pkg/astra_arm.py @@ -0,0 +1,92 @@ +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 + + +# 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, 0.0, angles[0], angles[1], angles[2], angles[3], angles[4], 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('ik_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: Ignore, Ignore, Ax_0, Ax_1, Ax2, Ax_3, Wrist, Ignore + self.zero_angles = [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 + + + # 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 + + 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] + + + + \ No newline at end of file From 85a231478e6822deb2fbbaeedb853629cdb4f97f Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 12 Apr 2025 11:42:13 -0500 Subject: [PATCH 03/49] feat: All feedback for socket, early ik implement --- src/arm_pkg/arm_pkg/arm_node.py | 128 +++++++++++++++++++++++-------- src/arm_pkg/arm_pkg/astra_arm.py | 18 ++++- 2 files changed, 113 insertions(+), 33 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index e008020..affeb04 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -19,15 +19,50 @@ 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 ikpy.chain import Chain +# from ikpy.link import OriginLink, URDFLink +# #import pygame as pyg +# from scipy.spatial.transform import Rotation as R +import astra_arm serial_pub = None thread = None + +# struct for socket feedback +class SocketFeedback: + def __init__(self): + self.bat_voltage = 0.0 + self.voltage_12 = 0.0 + self.voltage_5 = 0.0 + self.voltage_3 = 0.0 + self.joint_angles = [0.0, 0.0, 0.0, 0.0] + self.joint_temps = [0.0, 0.0, 0.0, 0.0] + self.joint_voltages = [0.0, 0.0, 0.0, 0.0] + self.joint_currents = [0.0, 0.0, 0.0, 0.0] + + def updateBusVoltages(self, voltages): + self.bat_voltage = voltages[0] + self.voltage_12 = voltages[1] + self.voltage_5 = voltages[2] + self.voltage_3 = voltages[3] + + def updateJointVoltages(self, axis, voltage): + self.joint_voltages[axis] = voltage + + def updateJointCurrents(self, axis, current): + self.joint_currents[axis] = current + + def updateJointTemperatures(self, axis, temperature): + self.joint_temps[axis] = temperature + + def updateJointAngles(self, angles): + self.joint_angles = angles + + +arm_feedback = SocketFeedback() + class SerialRelay(Node): def __init__(self): # Initialize node @@ -55,33 +90,7 @@ class SerialRelay(Node): self.arm_feedback = SocketFeedback() self.digit_feedback = DigitFeedback() - ######## - # Feedback and IK vars - ######## - # Misc - degree = pi / 180.0 - - urdf_file_name = 'arm11.urdf' - ik_tolerance = 1e-3 #Tolerance (in meters) to determine if solution is valid - - # URDF file path - urdf = os.path.join(get_package_share_directory('ik_pkg'), urdf_file_name) - # IKpy Chain - arm_chain = Chain.from_urdf_file(urdf) - - # Arrays for joint states - # Some links in the URDF are static (non-joints), these will remain zero for IK - # Indexes: Ignore, Ignore, Ax_0, Ax_1, Ax2, Ax_3, Wrist, Ignore - zero_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - current_angles = zero_angles - last_angles = zero_angles - ik_angles = zero_angles - - target_position = [0.0, 0.0, 0.0] - target_orientation = [] # Effector orientation desired at target position. - # Generally orientation for the effector is modified manually by the operator. - - + self.arm = astra_arm.Arm('arm11.urdf') ######## # Interface with MCU @@ -144,7 +153,49 @@ class SerialRelay(Node): try: output = str(self.ser.readline(), "utf8") if output: - #self.get_logger().info(f"[MCU] {output}") + if output.startswith("can_relay_tovic,arm,55"): + # Angle feedbacks + parts = output.split(",") + if len(parts) >= 7: + # Extract the angles from the string + angles_in = parts[3:7] + # Convert the angles to floats divide by 10.0 + angles = [float(angle) / 10.0 for angle in angles_in] + angles[0] = 0.0 + # Update the arm's current angles + self.arm.update_angles(angles) + else: + self.get_logger().info("Invalid angle feedback input format") + elif output.startswith("can_relay_tovic,arm,54"): + # Bus Voltage feedbacks + parts = output.split(",") + if len(parts) >= 7: + # Extract the voltage from the string + voltages_in = parts[3:7] + # Convert the voltages to floats + v_bat = float(voltages_in[0]) / 100.0 + v_12 = float(voltages_in[1]) / 100.0 + v_5 = float(voltages_in[2]) / 100.0 + v_3 = float(voltages_in[3]) / 100.0 + # Update the arm's current voltages + arm_feedback.updateBusVoltages([v_bat, v_12, v_5, v_3]) + else: + self.get_logger().info("Invalid voltage feedback input format") + elif output.startswith("can_relay_tovic,arm,53"): + # Motor voltage/current/temperature feedback + parts = output.split(",") + if len(parts) >= 7: + # Extract the voltage/current/temperature from the string + values_in = parts[3:7] + # Convert the voltages to floats + for i in range(4): + arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0) + arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0) + arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0) + else: + self.get_logger().info("Invalid motor feedback input format") + + self.get_logger().info(f"[MCU] {output}") msg = String() msg.data = output self.debug_pub.publish(msg) @@ -318,6 +369,19 @@ class SerialRelay(Node): else: self.get_logger().info("Invalid voltage feedback input format") + def socket_pub_callback(self): + # Create a SocketFeedback message and publish it + msg = SocketFeedback() + msg.bat_voltage = arm_feedback.bat_voltage + msg.voltage_12 = arm_feedback.voltage_12 + msg.voltage_5 = arm_feedback.voltage_5 + msg.voltage_3 = arm_feedback.voltage_3 + msg.joint_angles = arm_feedback.joint_angles + msg.joint_temps = arm_feedback.joint_temps + msg.joint_voltages = arm_feedback.joint_voltages + msg.joint_currents = arm_feedback.joint_currents + + self.socket_pub.publish(msg) def send_ik(self, msg): pass diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py index 9bc84fd..2f8002b 100644 --- a/src/arm_pkg/arm_pkg/astra_arm.py +++ b/src/arm_pkg/arm_pkg/astra_arm.py @@ -17,7 +17,7 @@ def convert_angles(angles): return [0.0, 0.0, angles[0], angles[1], angles[2], angles[3], angles[4], 0.0] -class arm: +class Arm: def __init__(self, urdf_name): self.ik_tolerance = 1e-3 #Tolerance (in meters) to determine if solution is valid # URDF file path @@ -43,6 +43,22 @@ class arm: #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() + try: + self.ik_angles = self.chain.inverse_kinematics( + target_position=self.target_position, + target_orientation=self.target_orientation, + initial_position=self.current_angles, + max_iterations=1000, + tolerance=self.ik_tolerance + ) + except Exception as e: + print(f"IK failed: {e}") + return False + # Get current orientation of the end effector and update target_orientation def update_orientation(self): From a4170344365ba4e1c1523c4b41aa7a65e34882c9 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 12 Apr 2025 11:59:16 -0500 Subject: [PATCH 04/49] fix: update socketFeedback() class --- src/arm_pkg/arm_pkg/arm_node.py | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index affeb04..811fa13 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -61,7 +61,7 @@ class SocketFeedback: self.joint_angles = angles -arm_feedback = SocketFeedback() + class SerialRelay(Node): def __init__(self): @@ -91,6 +91,7 @@ class SerialRelay(Node): self.arm_feedback = SocketFeedback() self.digit_feedback = DigitFeedback() self.arm = astra_arm.Arm('arm11.urdf') + self.arm_feedback = SocketFeedback() ######## # Interface with MCU @@ -178,7 +179,7 @@ class SerialRelay(Node): v_5 = float(voltages_in[2]) / 100.0 v_3 = float(voltages_in[3]) / 100.0 # Update the arm's current voltages - arm_feedback.updateBusVoltages([v_bat, v_12, v_5, v_3]) + self.arm_feedback.updateBusVoltages([v_bat, v_12, v_5, v_3]) else: self.get_logger().info("Invalid voltage feedback input format") elif output.startswith("can_relay_tovic,arm,53"): @@ -189,9 +190,9 @@ class SerialRelay(Node): values_in = parts[3:7] # Convert the voltages to floats for i in range(4): - arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0) - arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0) - arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0) + self.arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0) + self.arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0) + self.arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0) else: self.get_logger().info("Invalid motor feedback input format") @@ -372,14 +373,14 @@ class SerialRelay(Node): def socket_pub_callback(self): # Create a SocketFeedback message and publish it msg = SocketFeedback() - msg.bat_voltage = arm_feedback.bat_voltage - msg.voltage_12 = arm_feedback.voltage_12 - msg.voltage_5 = arm_feedback.voltage_5 - msg.voltage_3 = arm_feedback.voltage_3 - msg.joint_angles = arm_feedback.joint_angles - msg.joint_temps = arm_feedback.joint_temps - msg.joint_voltages = arm_feedback.joint_voltages - msg.joint_currents = arm_feedback.joint_currents + msg.bat_voltage = self.arm_feedback.bat_voltage + msg.voltage_12 = self.arm_feedback.voltage_12 + msg.voltage_5 = self.arm_feedback.voltage_5 + msg.voltage_3 = self.arm_feedback.voltage_3 + msg.joint_angles = self.arm_feedback.joint_angles + msg.joint_temps = self.arm_feedback.joint_temps + msg.joint_voltages = self.arm_feedback.joint_voltages + msg.joint_currents = self.arm_feedback.joint_currents self.socket_pub.publish(msg) From 9dacdfb385452548789a31e1824c3c1bfd0998a6 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 12 Apr 2025 12:05:15 -0500 Subject: [PATCH 05/49] refactor: feedback record helper functions --- src/arm_pkg/arm_pkg/arm_node.py | 86 ++++++++++++++++++--------------- 1 file changed, 47 insertions(+), 39 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 811fa13..b55498d 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -155,47 +155,11 @@ class SerialRelay(Node): output = str(self.ser.readline(), "utf8") if output: if output.startswith("can_relay_tovic,arm,55"): - # Angle feedbacks - parts = output.split(",") - if len(parts) >= 7: - # Extract the angles from the string - angles_in = parts[3:7] - # Convert the angles to floats divide by 10.0 - angles = [float(angle) / 10.0 for angle in angles_in] - angles[0] = 0.0 - # Update the arm's current angles - self.arm.update_angles(angles) - else: - self.get_logger().info("Invalid angle feedback input format") + self.recordAngleFeedback(output) elif output.startswith("can_relay_tovic,arm,54"): - # Bus Voltage feedbacks - parts = output.split(",") - if len(parts) >= 7: - # Extract the voltage from the string - voltages_in = parts[3:7] - # Convert the voltages to floats - v_bat = float(voltages_in[0]) / 100.0 - v_12 = float(voltages_in[1]) / 100.0 - v_5 = float(voltages_in[2]) / 100.0 - v_3 = float(voltages_in[3]) / 100.0 - # Update the arm's current voltages - self.arm_feedback.updateBusVoltages([v_bat, v_12, v_5, v_3]) - else: - self.get_logger().info("Invalid voltage feedback input format") + self.recordBusVoltage(output) elif output.startswith("can_relay_tovic,arm,53"): - # Motor voltage/current/temperature feedback - parts = output.split(",") - if len(parts) >= 7: - # Extract the voltage/current/temperature from the string - values_in = parts[3:7] - # Convert the voltages to floats - for i in range(4): - self.arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0) - self.arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0) - self.arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0) - else: - self.get_logger().info("Invalid motor feedback input format") - + self.recordMotorFeedback(output) self.get_logger().info(f"[MCU] {output}") msg = String() msg.data = output @@ -219,6 +183,50 @@ class SerialRelay(Node): pass + def recordAngleFeedback(self, msg): + # Angle feedbacks + parts = msg.split(",") + if len(parts) >= 7: + # Extract the angles from the string + angles_in = parts[3:7] + # Convert the angles to floats divide by 10.0 + angles = [float(angle) / 10.0 for angle in angles_in] + angles[0] = 0.0 + # Update the arm's current angles + self.arm.update_angles(angles) + else: + self.get_logger().info("Invalid angle feedback input format") + + def recordBusVoltage(self, msg): + # Bus Voltage feedbacks + parts = msg.split(",") + if len(parts) >= 7: + # Extract the voltage from the string + voltages_in = parts[3:7] + # Convert the voltages to floats + v_bat = float(voltages_in[0]) / 100.0 + v_12 = float(voltages_in[1]) / 100.0 + v_5 = float(voltages_in[2]) / 100.0 + v_3 = float(voltages_in[3]) / 100.0 + # Update the arm's current voltages + self.arm_feedback.updateBusVoltages([v_bat, v_12, v_5, v_3]) + else: + self.get_logger().info("Invalid voltage feedback input format") + + def recordMotorFeedback(self, msg): + # Motor voltage/current/temperature feedback + parts = msg.split(",") + if len(parts) >= 7: + # Extract the voltage/current/temperature from the string + values_in = parts[3:7] + # Convert the voltages to floats + for i in range(4): + self.arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0) + self.arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0) + self.arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0) + else: + self.get_logger().info("Invalid motor feedback input format") + def send_manual(self, msg: ArmManual): axis0 = msg.axis0 axis1 = -1 * msg.axis1 From f4a611567e69b21942b30d4d59a37de893b05ba9 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 12 Apr 2025 14:35:09 -0500 Subject: [PATCH 06/49] 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 From 5df3027fa045773dca9948bbf1eaafa26f46645d Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 12 Apr 2025 14:48:40 -0500 Subject: [PATCH 07/49] fix: anchor enables CAN Relay --- src/anchor_pkg/anchor_pkg/anchor_node.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 6b66ddb..5fc6466 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -45,6 +45,7 @@ class SerialRelay(Node): #(f"Checking port {port}...") ser.write(b"ping\n") response = ser.read_until("\n") + ser.write(b"can_relay_mode,on\n") # if pong is in response, then we are talking with the MCU if b"pong" in response: From 5fc704dbc4771a4494a4c8beee5018996d91d195 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 12 Apr 2025 14:49:07 -0500 Subject: [PATCH 08/49] fix: proper astra_arm import for arm_node --- src/arm_pkg/arm_pkg/arm_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 736ce5a..8fb3700 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -24,7 +24,7 @@ from ament_index_python.packages import get_package_share_directory # #import pygame as pyg # from scipy.spatial.transform import Rotation as R -import astra_arm +from . import astra_arm serial_pub = None thread = None From 99916b317f7f1c2f1d07d02b9f2b24a141dfba08 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 12 Apr 2025 14:54:33 -0500 Subject: [PATCH 09/49] temp: raw angle output arm_node --- src/arm_pkg/arm_pkg/arm_node.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 8fb3700..b4d5ad3 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -194,6 +194,7 @@ class SerialRelay(Node): angles[0] = 0.0 # Update the arm's current angles self.arm.update_angles(angles) + self.get_logger().info(f"Angles: {angles}") else: self.get_logger().info("Invalid angle feedback input format") From b6348e4c00fd7b85f90dbf89b6fb13dc723b130b Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 12 Apr 2025 14:58:57 -0500 Subject: [PATCH 10/49] temp: print statements --- src/arm_pkg/arm_pkg/arm_node.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index b4d5ad3..36f803a 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -68,6 +68,9 @@ class SerialRelay(Node): # Initialize node super().__init__("arm_node") + #output to console + self.get_logger().info("Starting arm node...") + # Get launch mode parameter self.declare_parameter('launch_mode', 'arm') self.launch_mode = self.get_parameter('launch_mode').value @@ -88,6 +91,10 @@ class SerialRelay(Node): self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10) self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10) + + # output to console + self.get_logger().info("Creating arm object...") + self.arm_feedback = SocketFeedback() self.digit_feedback = DigitFeedback() self.arm = astra_arm.Arm('arm11.urdf') From e059f5cfecfca7ddd41c63948785fff9cc748627 Mon Sep 17 00:00:00 2001 From: ASTRA-SHC Date: Sat, 12 Apr 2025 16:05:19 -0500 Subject: [PATCH 11/49] nuking socket feedback class --- src/arm_pkg/arm_pkg/arm_node.py | 35 ++----------------------- src/arm_pkg/arm_pkg/astra_arm.py | 2 +- src/arm_pkg/arm_pkg/feedback_storage.py | 29 ++++++++++++++++++++ 3 files changed, 32 insertions(+), 34 deletions(-) create mode 100644 src/arm_pkg/arm_pkg/feedback_storage.py diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 36f803a..531d372 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -25,41 +25,12 @@ from ament_index_python.packages import get_package_share_directory # from scipy.spatial.transform import Rotation as R from . import astra_arm +#from . import socket_feedback serial_pub = None thread = None -# struct for socket feedback -class SocketFeedback: - def __init__(self): - self.bat_voltage = 0.0 - self.voltage_12 = 0.0 - self.voltage_5 = 0.0 - self.voltage_3 = 0.0 - self.joint_angles = [0.0, 0.0, 0.0, 0.0] - self.joint_temps = [0.0, 0.0, 0.0, 0.0] - self.joint_voltages = [0.0, 0.0, 0.0, 0.0] - self.joint_currents = [0.0, 0.0, 0.0, 0.0] - - def updateBusVoltages(self, voltages): - self.bat_voltage = voltages[0] - self.voltage_12 = voltages[1] - self.voltage_5 = voltages[2] - self.voltage_3 = voltages[3] - - def updateJointVoltages(self, axis, voltage): - self.joint_voltages[axis] = voltage - - def updateJointCurrents(self, axis, current): - self.joint_currents[axis] = current - - def updateJointTemperatures(self, axis, temperature): - self.joint_temps[axis] = temperature - - def updateJointAngles(self, angles): - self.joint_angles = angles - @@ -68,9 +39,6 @@ class SerialRelay(Node): # Initialize node super().__init__("arm_node") - #output to console - self.get_logger().info("Starting arm node...") - # Get launch mode parameter self.declare_parameter('launch_mode', 'arm') self.launch_mode = self.get_parameter('launch_mode').value @@ -230,6 +198,7 @@ class SerialRelay(Node): # Convert the voltages to floats for i in range(4): self.arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0) + self. self.arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0) self.arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0) else: diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py index 80bbe61..df89790 100644 --- a/src/arm_pkg/arm_pkg/astra_arm.py +++ b/src/arm_pkg/arm_pkg/astra_arm.py @@ -21,7 +21,7 @@ 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('ik_pkg'), urdf_name) + self.urdf = os.path.join(get_package_share_directory('arm_pkg'), urdf_name) # IKpy Chain self.chain = Chain.from_urdf_file(self.urdf) diff --git a/src/arm_pkg/arm_pkg/feedback_storage.py b/src/arm_pkg/arm_pkg/feedback_storage.py new file mode 100644 index 0000000..66c88d3 --- /dev/null +++ b/src/arm_pkg/arm_pkg/feedback_storage.py @@ -0,0 +1,29 @@ +# struct for socket feedback +class Feedback: + def __init__(self): + self.bat_voltage = 0.0 + self.voltage_12 = 0.0 + self.voltage_5 = 0.0 + self.voltage_3 = 0.0 + self.joint_angles = [0.0, 0.0, 0.0, 0.0] + self.joint_temps = [0.0, 0.0, 0.0, 0.0] + self.joint_voltages = [0.0, 0.0, 0.0, 0.0] + self.joint_currents = [0.0, 0.0, 0.0, 0.0] + + def updateBusVoltages(self, voltages): + self.bat_voltage = voltages[0] + self.voltage_12 = voltages[1] + self.voltage_5 = voltages[2] + self.voltage_3 = voltages[3] + + def updateJointVoltages(self, axis, voltage): + self.joint_voltages[axis] = voltage + + def updateJointCurrents(self, axis, current): + self.joint_currents[axis] = current + + def updateJointTemperatures(self, axis, temperature): + self.joint_temps[axis] = temperature + + def updateJointAngles(self, angles): + self.joint_angles = angles From f12daff861abfa9c6d0c18558c922d1e0a03cbe6 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 12 Apr 2025 16:25:46 -0500 Subject: [PATCH 12/49] feat: fully remove socket_feedback class --- src/arm_pkg/arm_pkg/arm_node.py | 46 +++++++++++++------------ src/arm_pkg/arm_pkg/feedback_storage.py | 29 ---------------- 2 files changed, 24 insertions(+), 51 deletions(-) delete mode 100644 src/arm_pkg/arm_pkg/feedback_storage.py diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 531d372..f0d7201 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -25,7 +25,6 @@ from ament_index_python.packages import get_package_share_directory # from scipy.spatial.transform import Rotation as R from . import astra_arm -#from . import socket_feedback serial_pub = None thread = None @@ -33,7 +32,6 @@ thread = None - class SerialRelay(Node): def __init__(self): # Initialize node @@ -169,6 +167,10 @@ class SerialRelay(Node): angles[0] = 0.0 # Update the arm's current angles self.arm.update_angles(angles) + self.arm_feedback.axis0_angle = angles[0] + self.arm_feedback.axis1_angle = angles[1] + self.arm_feedback.axis2_angle = angles[2] + self.arm_feedback.axis3_angle = angles[3] self.get_logger().info(f"Angles: {angles}") else: self.get_logger().info("Invalid angle feedback input format") @@ -180,12 +182,10 @@ class SerialRelay(Node): # Extract the voltage from the string voltages_in = parts[3:7] # Convert the voltages to floats - v_bat = float(voltages_in[0]) / 100.0 - v_12 = float(voltages_in[1]) / 100.0 - v_5 = float(voltages_in[2]) / 100.0 - v_3 = float(voltages_in[3]) / 100.0 - # Update the arm's current voltages - self.arm_feedback.updateBusVoltages([v_bat, v_12, v_5, v_3]) + 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") @@ -197,10 +197,12 @@ class SerialRelay(Node): values_in = parts[3:7] # Convert the voltages to floats for i in range(4): - self.arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0) - self. - self.arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0) - self.arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0) + #update arm_feedback's axisX_temp for each axis0_temp, axis1_temp, etc... + pass + + # self.arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0) + # self.arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0) + # self.arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0) else: self.get_logger().info("Invalid motor feedback input format") @@ -357,17 +359,17 @@ class SerialRelay(Node): def socket_pub_callback(self): # Create a SocketFeedback message and publish it - msg = SocketFeedback() - msg.bat_voltage = self.arm_feedback.bat_voltage - msg.voltage_12 = self.arm_feedback.voltage_12 - msg.voltage_5 = self.arm_feedback.voltage_5 - msg.voltage_3 = self.arm_feedback.voltage_3 - msg.joint_angles = self.arm_feedback.joint_angles - msg.joint_temps = self.arm_feedback.joint_temps - msg.joint_voltages = self.arm_feedback.joint_voltages - msg.joint_currents = self.arm_feedback.joint_currents + # msg = SocketFeedback() + # msg.bat_voltage = self.arm_feedback.bat_voltage + # msg.voltage_12 = self.arm_feedback.voltage_12 + # msg.voltage_5 = self.arm_feedback.voltage_5 + # msg.voltage_3 = self.arm_feedback.voltage_3 + # msg.joint_angles = self.arm_feedback.joint_angles + # msg.joint_temps = self.arm_feedback.joint_temps + # msg.joint_voltages = self.arm_feedback.joint_voltages + # msg.joint_currents = self.arm_feedback.joint_currents - self.socket_pub.publish(msg) #Publish feedback from arm + self.socket_pub.publish(self.arm_feedback) #Publish feedback from arm self.arm.update_position() #Run FK and update the current position of the arm, using FK diff --git a/src/arm_pkg/arm_pkg/feedback_storage.py b/src/arm_pkg/arm_pkg/feedback_storage.py deleted file mode 100644 index 66c88d3..0000000 --- a/src/arm_pkg/arm_pkg/feedback_storage.py +++ /dev/null @@ -1,29 +0,0 @@ -# struct for socket feedback -class Feedback: - def __init__(self): - self.bat_voltage = 0.0 - self.voltage_12 = 0.0 - self.voltage_5 = 0.0 - self.voltage_3 = 0.0 - self.joint_angles = [0.0, 0.0, 0.0, 0.0] - self.joint_temps = [0.0, 0.0, 0.0, 0.0] - self.joint_voltages = [0.0, 0.0, 0.0, 0.0] - self.joint_currents = [0.0, 0.0, 0.0, 0.0] - - def updateBusVoltages(self, voltages): - self.bat_voltage = voltages[0] - self.voltage_12 = voltages[1] - self.voltage_5 = voltages[2] - self.voltage_3 = voltages[3] - - def updateJointVoltages(self, axis, voltage): - self.joint_voltages[axis] = voltage - - def updateJointCurrents(self, axis, current): - self.joint_currents[axis] = current - - def updateJointTemperatures(self, axis, temperature): - self.joint_temps[axis] = temperature - - def updateJointAngles(self, angles): - self.joint_angles = angles From 8c4f75f75e4cd303c97a37d6b12704a896fdaea2 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Tue, 15 Apr 2025 20:35:49 -0500 Subject: [PATCH 13/49] feat: nodes should die entirely if no MCU found This should allow anchor to restart automatically if it's being run as a service --- src/anchor_pkg/anchor_pkg/anchor_node.py | 6 ++++-- src/arm_pkg/arm_pkg/arm_node.py | 7 ++++--- src/bio_pkg/bio_pkg/bio_node.py | 8 +++++--- src/core_pkg/core_pkg/core_node.py | 6 ++++-- 4 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 5fc6466..e3dd1d9 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -7,6 +7,7 @@ import time import atexit import serial +import os import sys import threading import glob @@ -61,8 +62,9 @@ class SerialRelay(Node): if self.port is None: self.get_logger().info("Unable to find MCU...") - time.sleep(1) - sys.exit(1) + #kill the node/process entirely + os.kill(os.getpid(), signal.SIGKILL) + sys.exit(0) self.ser = serial.Serial(self.port, 115200) atexit.register(self.cleanup) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index f0d7201..3a2e9d7 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -95,9 +95,10 @@ class SerialRelay(Node): break if self.port is None: - self.get_logger().info("Unable to find MCU... please make sure it is connected.") - time.sleep(1) - sys.exit(1) + self.get_logger().info("Unable to find MCU...") + #kill the node/process entirely + os.kill(os.getpid(), signal.SIGKILL) + sys.exit(0) self.ser = serial.Serial(self.port, 115200) atexit.register(self.cleanup) diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index ad8f40b..5518b87 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -3,6 +3,7 @@ from rclpy.node import Node import serial import sys import threading +import os import glob import time import atexit @@ -65,9 +66,10 @@ class SerialRelay(Node): pass if self.port is None: - self.get_logger().info("Unable to find MCU... please make sure it is connected.") - time.sleep(1) - sys.exit(1) + self.get_logger().info("Unable to find MCU...") + #kill the node/process entirely + os.kill(os.getpid(), signal.SIGKILL) + sys.exit(0) self.ser = serial.Serial(self.port, 115200) atexit.register(self.cleanup) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 1768293..f1398a0 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -7,6 +7,7 @@ import time import atexit import serial +import os import sys import threading import glob @@ -74,8 +75,9 @@ class SerialRelay(Node): if self.port is None: self.get_logger().info("Unable to find MCU...") - time.sleep(1) - sys.exit(1) + #kill the node/process entirely + os.kill(os.getpid(), signal.SIGKILL) + sys.exit(0) self.ser = serial.Serial(self.port, 115200) atexit.register(self.cleanup) From 0c3f27667a02f2860c3f37359ab42473682cf91a Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Tue, 15 Apr 2025 20:42:36 -0500 Subject: [PATCH 14/49] SIGSTP no longer does sys.exit() --- src/anchor_pkg/anchor_pkg/anchor_node.py | 2 +- src/arm_pkg/arm_pkg/arm_node.py | 2 +- src/bio_pkg/bio_pkg/bio_node.py | 2 +- src/core_pkg/core_pkg/core_node.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index e3dd1d9..575870b 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -150,6 +150,6 @@ def main(args=None): serial_pub.run() if __name__ == '__main__': - signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly + #signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0)) # Catch termination signals and exit cleanly main() diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 3a2e9d7..06a11c2 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -450,6 +450,6 @@ def main(args=None): serial_pub.run() if __name__ == '__main__': - signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly + #signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0)) # Catch termination signals and exit cleanly main() diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index 5518b87..5d6fa7a 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -221,6 +221,6 @@ def main(args=None): serial_pub.run() if __name__ == '__main__': - signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly + #signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0)) # Catch termination signals and exit cleanly main() diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index f1398a0..baaaa2d 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -285,7 +285,7 @@ def main(args=None): serial_pub.run() if __name__ == '__main__': - signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly + #signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0)) # Catch termination signals and exit cleanly main() From 9826b39e7eaa4d44a34b9807e4b3fb9ec920a151 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Wed, 16 Apr 2025 20:01:00 -0500 Subject: [PATCH 15/49] Fix: arm feedback through anchor --- src/arm_pkg/arm_pkg/arm_node.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 06a11c2..8fcbcc6 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -128,11 +128,11 @@ class SerialRelay(Node): try: output = str(self.ser.readline(), "utf8") if output: - if output.startswith("can_relay_tovic,arm,55"): + if output.startswith("can_relay_fromvic,arm,55"): self.recordAngleFeedback(output) - elif output.startswith("can_relay_tovic,arm,54"): + elif output.startswith("can_relay_fromvic,arm,54"): self.recordBusVoltage(output) - elif output.startswith("can_relay_tovic,arm,53"): + elif output.startswith("can_relay_fromvic,arm,53"): self.recordMotorFeedback(output) self.get_logger().info(f"[MCU] {output}") msg = String() From 8e3f2ee88a6604c40fb59fa93272dac999c6eec2 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Wed, 16 Apr 2025 20:03:54 -0500 Subject: [PATCH 16/49] fix: anchor publishes feedback for respective feedback topics --- src/anchor_pkg/anchor_pkg/anchor_node.py | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 575870b..b21da4a 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -88,7 +88,22 @@ class SerialRelay(Node): if output: # All output over debug temporarily - #self.get_logger().info(f"[MCU] {output}") + 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 if output.startswith("can_relay_fromvic,core"): From 27cf4d99829b1e945f70d766793e478cdc1c5d75 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Wed, 16 Apr 2025 20:06:51 -0500 Subject: [PATCH 17/49] debug print for arm_node socket_pub_callback --- src/arm_pkg/arm_pkg/arm_node.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 8fcbcc6..2ebaa8b 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -369,7 +369,8 @@ class SerialRelay(Node): # msg.joint_temps = self.arm_feedback.joint_temps # msg.joint_voltages = self.arm_feedback.joint_voltages # msg.joint_currents = self.arm_feedback.joint_currents - + #debug print + self.get_logger().info(f"ARM NODE SENDING: Socket Feedback") self.socket_pub.publish(self.arm_feedback) #Publish feedback from arm self.arm.update_position() #Run FK and update the current position of the arm, using FK From f00a7d21ce33980c0e95ec1cd11d9aeb7cb2b152 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Wed, 16 Apr 2025 20:32:28 -0500 Subject: [PATCH 18/49] Fix: arm angle feedback working --- src/arm_pkg/arm_pkg/arm_node.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 2ebaa8b..38bbe22 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -166,6 +166,24 @@ class SerialRelay(Node): # Convert the angles to floats divide by 10.0 angles = [float(angle) / 10.0 for angle in angles_in] angles[0] = 0.0 + # + # + # + #THIS NEEDS TO BE REMOVED LATER + #PLACEHOLDER FOR WRIST VALUE + # + ## + # + # + # + angles.append(0.0) + # + # + # + # + # + ## + # # Update the arm's current angles self.arm.update_angles(angles) self.arm_feedback.axis0_angle = angles[0] From 4420e83981eb4a437c2c493029dda332b2648595 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Wed, 16 Apr 2025 20:36:35 -0500 Subject: [PATCH 19/49] remove debug print for arm_node --- src/arm_pkg/arm_pkg/arm_node.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 38bbe22..b15199a 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -388,7 +388,6 @@ class SerialRelay(Node): # msg.joint_voltages = self.arm_feedback.joint_voltages # msg.joint_currents = self.arm_feedback.joint_currents #debug print - self.get_logger().info(f"ARM NODE SENDING: Socket Feedback") self.socket_pub.publish(self.arm_feedback) #Publish feedback from arm self.arm.update_position() #Run FK and update the current position of the arm, using FK From 3985c11ae291c3267558d5e0f9da0d7a981c01ca Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Fri, 18 Apr 2025 13:34:53 -0500 Subject: [PATCH 20/49] Added debug output to /arm/feedback/debug for testing IK --- src/arm_pkg/arm_pkg/arm_node.py | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index b15199a..3cffcbb 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -136,7 +136,7 @@ class SerialRelay(Node): self.recordMotorFeedback(output) self.get_logger().info(f"[MCU] {output}") msg = String() - msg.data = output + msg.data = "From MCU Got: " + output self.debug_pub.publish(msg) except serial.SerialException: self.get_logger().info("SerialException caught... closing serial port.") @@ -397,19 +397,39 @@ class SerialRelay(Node): def send_ik(self, msg): input_raw = msg.movement_vector # [x, y, z] + # Debug output + msg = String() + msg.data = "From IK Control Got Vector: " + str(input_raw) + self.debug_pub.publish(msg) + # 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 + msg.data = "Target Position: " + str(target_position) + self.debug_pub.publish(msg) 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}") + + msg = String() + msg.data = "IK Success: " + str(target_position) + 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 From dde9d61a3337ad5cdf713c50b21cb9f55896ca31 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 19 Apr 2025 11:46:36 -0500 Subject: [PATCH 21/49] fix formatting for anchor print statements in arm and core --- src/arm_pkg/arm_pkg/arm_node.py | 2 +- src/core_pkg/core_pkg/core_node.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 3cffcbb..4ebacf8 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -273,7 +273,7 @@ class SerialRelay(Node): output.data = msg self.anchor_pub.publish(output) elif self.launch_mode == 'arm': #if in standalone mode, send to MCU directly - self.get_logger().info(f"[Arm to MCU] {msg}") + self.get_logger().info(f"[Arm to MCU] {msg.data}") self.ser.write(bytes(msg, "utf8")) def anchor_feedback(self, msg: String): diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index baaaa2d..ac50a69 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -191,7 +191,7 @@ class SerialRelay(Node): output.data = msg self.anchor_pub.publish(output) elif self.launch_mode == 'core': - self.get_logger().info(f"[Core to MCU] {msg}") + self.get_logger().info(f"[Core to MCU] {msg.data}") self.ser.write(bytes(msg, "utf8")) def anchor_feedback(self, msg: String): From 5c41d66404965f27cef2a8ae8f8b9770f849e8bb Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 26 Apr 2025 12:02:30 -0500 Subject: [PATCH 22/49] debug printing --- src/arm_pkg/arm_pkg/arm_node.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 4ebacf8..60d4273 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -396,11 +396,15 @@ 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] # Debug output - msg = String() - msg.data = "From IK Control Got Vector: " + str(input_raw) - self.debug_pub.publish(msg) + 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 @@ -410,8 +414,8 @@ class SerialRelay(Node): #Target position is current position + normalized vector target_position = self.arm.get_position() + input_norm - msg.data = "Target Position: " + str(target_position) - self.debug_pub.publish(msg) + 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 From c3026265123defe775b0d579d2bb9c8a06719056 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 26 Apr 2025 12:04:38 -0500 Subject: [PATCH 23/49] remove vector normalization- done on base station --- src/arm_pkg/arm_pkg/arm_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 60d4273..db31fe8 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -407,13 +407,13 @@ class SerialRelay(Node): self.debug_pub.publish(tempMsg) # normalize the vector - input_norm = np.linalg.norm(input_raw) / 2.0 + #input_norm = np.linalg.norm(input_raw) / 2.0 #Target position is current position + normalized vector - target_position = self.arm.get_position() + input_norm + target_position = self.arm.get_position() + input_raw tempMsg.data = "Target Position: " + str(target_position) self.debug_pub.publish(tempMsg) From 5eb9e8a2e3e76fc4e7bb2fe45c1ef92f2fa73c25 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 26 Apr 2025 12:08:46 -0500 Subject: [PATCH 24/49] add get_position_vector3() --- src/arm_pkg/arm_pkg/arm_node.py | 2 +- src/arm_pkg/arm_pkg/astra_arm.py | 21 +++++++++++++++++++-- 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index db31fe8..f44766f 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -413,7 +413,7 @@ class SerialRelay(Node): #Target position is current position + normalized vector - target_position = self.arm.get_position() + input_raw + target_position = self.arm.get_position_vector() + input_raw tempMsg.data = "Target Position: " + str(target_position) self.debug_pub.publish(tempMsg) diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py index df89790..7d3cb24 100644 --- a/src/arm_pkg/arm_pkg/astra_arm.py +++ b/src/arm_pkg/arm_pkg/astra_arm.py @@ -6,6 +6,7 @@ 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 @@ -104,6 +105,23 @@ class Arm: 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] + + # 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 + def update_position(self): # FK matrix for arm's current pose @@ -113,5 +131,4 @@ class Arm: self.current_position = fk_matrix[:3, 3] - - \ No newline at end of file + From 3d18e209463e2b97b9f52eb34f0b7f260b65a236 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 26 Apr 2025 12:10:33 -0500 Subject: [PATCH 25/49] More fixes for arm_node.py --- src/arm_pkg/arm_pkg/arm_node.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index f44766f..29e2379 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -413,7 +413,8 @@ class SerialRelay(Node): #Target position is current position + normalized vector - target_position = self.arm.get_position_vector() + input_raw + #target_position = self.arm.get_position_vector() + input_raw + target_position = map(sum, zip(self.arm.get_position_vector(), input_raw)) tempMsg.data = "Target Position: " + str(target_position) self.debug_pub.publish(tempMsg) From 60952db588f6a425a14e356c025a602abf2dc760 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 26 Apr 2025 12:17:10 -0500 Subject: [PATCH 26/49] 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): From 28604635016590bca912114cb58eb3782f845a84 Mon Sep 17 00:00:00 2001 From: ASTRA-SHC Date: Sat, 26 Apr 2025 13:04:20 -0500 Subject: [PATCH 27/49] fixes and debug to ik- need to update the model --- src/arm_pkg/arm_pkg/arm_node.py | 39 ++++++++++++++++++-------------- src/arm_pkg/arm_pkg/astra_arm.py | 23 ++++++++++++++++--- 2 files changed, 42 insertions(+), 20 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 980015d..3693626 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -191,6 +191,10 @@ class SerialRelay(Node): self.arm_feedback.axis2_angle = angles[2] self.arm_feedback.axis3_angle = angles[3] self.get_logger().info(f"Angles: {angles}") + #debug publish angles + tempMsg = String() + tempMsg.data = "Angles: " + str(angles) + self.debug_pub.publish(tempMsg) else: self.get_logger().info("Invalid angle feedback input format") @@ -395,7 +399,10 @@ class SerialRelay(Node): def send_ik(self, msg): - input_raw = np.array(msg.movement_vector) # Convert input to a NumPy array + # Convert Vector3 to a NumPy array + input_raw = np.array([msg.movement_vector.x, msg.movement_vector.y, msg.movement_vector.z]) # Convert input to a NumPy array + # decrease input vector by 90% + input_raw = input_raw * 0.2 # Debug output tempMsg = String() @@ -406,6 +413,11 @@ class SerialRelay(Node): current_position = self.arm.get_position_vector() target_position = current_position + input_raw + + # Debug output for current position + tempMsg.data = "Current Position: " + str(current_position) + self.debug_pub.publish(tempMsg) + # Debug output for target position tempMsg.data = "Target Position: " + str(target_position) self.debug_pub.publish(tempMsg) @@ -417,16 +429,16 @@ class SerialRelay(Node): self.send_cmd(command) self.get_logger().info(f"IK Success: {target_position}") - msg = String() - msg.data = "IK Success: " + str(target_position) - self.debug_pub.publish(msg) - msg.data = "Sending: " + str(command) - self.debug_pub.publish(msg) + tempMsg = String() + tempMsg.data = "IK Success: " + str(target_position) + self.debug_pub.publish(tempMsg) + tempMsg.data = "Sending: " + str(command) + self.debug_pub.publish(tempMsg) else: self.get_logger().info("IK Fail") - msg = String() - msg.data = "IK Fail" - self.debug_pub.publish(msg) + tempMsg = String() + tempMsg.data = "IK Fail" + self.debug_pub.publish(tempMsg) # Manual control for Wrist/Effector command = "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n" @@ -449,14 +461,7 @@ class SerialRelay(Node): pass - # Given the FK_Matix for the arm's current pose, update the orientation array - def update_orientation(self, fk_matrix): - self.target_orientation = fk_matrix[:3, :3] - return - - def update_joints(self, ax_0, ax_1, ax_2, ax_3, wrist): - self.current_angles = [0.0, 0.0, ax_0, ax_1, ax_2, ax_3, wrist, 0.0] - return + diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py index e9d4a84..02e509b 100644 --- a/src/arm_pkg/arm_pkg/astra_arm.py +++ b/src/arm_pkg/arm_pkg/astra_arm.py @@ -48,7 +48,11 @@ class Arm: 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, @@ -57,18 +61,31 @@ class Arm: ) # 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: - self.get_logger().info(f"No VALID IK Solution within tolerance. Error: {error}") + print(f"No VALID IK Solution within tolerance. Error: {error}") return False else: - self.get_logger().info(f"IK Solution Found. Error: {error}") + print(f"IK Solution Found. Error: {error}") return True except Exception as e: - print(f"IK failed: {e}") + print(f"IK failed for exception: {e}") return False + # # Given the FK_Matix for the arm's current pose, update the orientation array + # def update_orientation(self, fk_matrix): + # self.target_orientation = fk_matrix[:3, :3] + # return + + # def update_joints(self, ax_0, ax_1, ax_2, ax_3, wrist): + # self.current_angles = [0.0, 0.0, ax_0, ax_1, ax_2, ax_3, wrist, 0.0] + # return + # Get current orientation of the end effector and update target_orientation def update_orientation(self): From c9533e3f55ca1f71c20fd58c618fe6f7d0e16118 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 3 May 2025 12:43:17 -0500 Subject: [PATCH 28/49] Convert angles to radians for IK --- src/arm_pkg/arm_pkg/arm_node.py | 11 +- src/arm_pkg/arm_pkg/astra_arm.py | 16 +- src/arm_pkg/urdf/arm12.urdf | 307 +++++++++++++++++++++++++++++++ 3 files changed, 316 insertions(+), 18 deletions(-) create mode 100644 src/arm_pkg/urdf/arm12.urdf diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 3693626..23bbf62 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -63,7 +63,7 @@ class SerialRelay(Node): self.arm_feedback = SocketFeedback() self.digit_feedback = DigitFeedback() - self.arm = astra_arm.Arm('arm11.urdf') + self.arm = astra_arm.Arm('arm12.urdf') self.arm_feedback = SocketFeedback() ######## @@ -168,22 +168,13 @@ class SerialRelay(Node): angles[0] = 0.0 # # - # #THIS NEEDS TO BE REMOVED LATER #PLACEHOLDER FOR WRIST VALUE # - ## - # - # # angles.append(0.0) # # - # - # - # - ## - # # Update the arm's current angles self.arm.update_angles(angles) self.arm_feedback.axis0_angle = angles[0] diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py index 02e509b..5dd835e 100644 --- a/src/arm_pkg/arm_pkg/astra_arm.py +++ b/src/arm_pkg/arm_pkg/astra_arm.py @@ -15,7 +15,7 @@ degree = pi / 180.0 def convert_angles(angles): # Converts angles to the format used for the urdf (contains some dummy joints) - return [0.0, 0.0, angles[0], angles[1], angles[2], angles[3], angles[4], 0.0] + 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] class Arm: @@ -29,8 +29,8 @@ class Arm: # Arrays for joint states # Some links in the URDF are static (non-joints), these will remain zero for IK - # Indexes: Ignore, Ignore, Ax_0, Ax_1, Ax2, Ax_3, Wrist, Ignore - self.zero_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + # Indexes: Fixed_base, Ax_0, Ax_1, seg1, Ax_2, seg2, ax_3, seg3, continuous, wrist + self.zero_angles = [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 @@ -48,11 +48,11 @@ class Arm: 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}") + # 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}") + # 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, @@ -64,7 +64,7 @@ class Arm: fk_position = fk_matrix[:3, 3] - print(f"[TRY] FK Position for Solution: {fk_position}") + # print(f"[TRY] FK Position for Solution: {fk_position}") error = np.linalg.norm(target_position - fk_position) if error > self.ik_tolerance: diff --git a/src/arm_pkg/urdf/arm12.urdf b/src/arm_pkg/urdf/arm12.urdf new file mode 100644 index 0000000..6adb724 --- /dev/null +++ b/src/arm_pkg/urdf/arm12.urdf @@ -0,0 +1,307 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 8b9b72e78fd6a3ed8dfd0f1443bd2796d0c5968a Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 3 May 2025 12:48:01 -0500 Subject: [PATCH 29/49] Add dummy link for effector --- src/arm_pkg/arm_pkg/astra_arm.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py index 5dd835e..c0bcd9e 100644 --- a/src/arm_pkg/arm_pkg/astra_arm.py +++ b/src/arm_pkg/arm_pkg/astra_arm.py @@ -15,7 +15,7 @@ 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] + 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: From 9f8f51b742f09c7ebc8838e6ce0b2861b69144b6 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 3 May 2025 12:50:12 -0500 Subject: [PATCH 30/49] Fix: missed a spot --- src/arm_pkg/arm_pkg/astra_arm.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py index c0bcd9e..a61bd3a 100644 --- a/src/arm_pkg/arm_pkg/astra_arm.py +++ b/src/arm_pkg/arm_pkg/astra_arm.py @@ -29,8 +29,8 @@ class Arm: # 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 - self.zero_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + # 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 From 6a747f92fbaf0bfed3beadff3bb1c59027acdcb8 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 3 May 2025 12:53:43 -0500 Subject: [PATCH 31/49] Fix: Add more dummy values to support new urdf --- src/arm_pkg/arm_pkg/arm_node.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 23bbf62..1634742 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -172,7 +172,8 @@ class SerialRelay(Node): #PLACEHOLDER FOR WRIST VALUE # # - angles.append(0.0) + angles.append(0.0)#placeholder for wrist_continuous + angles.append(0.0)#placeholder for wrist # # # Update the arm's current angles From 923dfa20ca84da811e14b6a890547a4382647758 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 3 May 2025 13:05:56 -0500 Subject: [PATCH 32/49] remove all debug publishing for arm --- src/arm_pkg/arm_pkg/arm_node.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 1634742..fb47d7a 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -186,7 +186,7 @@ class SerialRelay(Node): #debug publish angles tempMsg = String() tempMsg.data = "Angles: " + str(angles) - self.debug_pub.publish(tempMsg) + #self.debug_pub.publish(tempMsg) else: self.get_logger().info("Invalid angle feedback input format") @@ -399,7 +399,7 @@ class SerialRelay(Node): # Debug output tempMsg = String() tempMsg.data = "From IK Control Got Vector: " + str(input_raw) - self.debug_pub.publish(tempMsg) + #self.debug_pub.publish(tempMsg) # Target position is current position + input vector current_position = self.arm.get_position_vector() @@ -408,11 +408,11 @@ class SerialRelay(Node): # Debug output for current position tempMsg.data = "Current Position: " + str(current_position) - self.debug_pub.publish(tempMsg) + #self.debug_pub.publish(tempMsg) # Debug output for target position tempMsg.data = "Target Position: " + str(target_position) - self.debug_pub.publish(tempMsg) + #self.debug_pub.publish(tempMsg) # Perform IK with the target position if self.arm.perform_ik(target_position): @@ -423,14 +423,14 @@ class SerialRelay(Node): tempMsg = String() tempMsg.data = "IK Success: " + str(target_position) - self.debug_pub.publish(tempMsg) + #self.debug_pub.publish(tempMsg) tempMsg.data = "Sending: " + str(command) - self.debug_pub.publish(tempMsg) + #self.debug_pub.publish(tempMsg) else: self.get_logger().info("IK Fail") tempMsg = String() tempMsg.data = "IK Fail" - self.debug_pub.publish(tempMsg) + #self.debug_pub.publish(tempMsg) # Manual control for Wrist/Effector command = "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n" From 482bedbfafe69e550fefd0aa16a0e185d0768bae Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 3 May 2025 13:12:15 -0500 Subject: [PATCH 33/49] debug print for manual control --- src/arm_pkg/arm_pkg/arm_node.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index fb47d7a..fd17209 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -434,6 +434,7 @@ class SerialRelay(Node): # 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" From 53b4259ade553b1cb31648dbb3ce97555329756f Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 3 May 2025 13:22:59 -0500 Subject: [PATCH 34/49] disable feedback --- src/arm_pkg/arm_pkg/arm_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index fd17209..17bdc99 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -54,7 +54,7 @@ class SerialRelay(Node): # Topics used in anchor mode if self.launch_mode == 'anchor': - self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10) + #self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10) self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10) From 43fdc7587a05954f964f35fbb9181e97b2039196 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 3 May 2025 13:31:06 -0500 Subject: [PATCH 35/49] send manual control debug print --- src/arm_pkg/arm_pkg/arm_node.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 17bdc99..f9110f5 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -227,6 +227,11 @@ class SerialRelay(Node): axis2 = msg.axis2 axis3 = msg.axis3 + tempMsg = String() + tempMsg.data = "Sending manual" + self.debug_pub.publish(tempMsg) + + #Send controls for arm command = "can_relay_tovic,arm,18," + str(int(msg.brake)) + "\n" command += "can_relay_tovic,arm,39," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n" From 0ea4c73876c85edbbcfe93030f5845b67cb55f70 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 3 May 2025 17:17:05 -0500 Subject: [PATCH 36/49] comment out some stuff to test delays --- src/arm_pkg/arm_pkg/arm_node.py | 31 +++++++++++++++++-------------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index f9110f5..866bfc6 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -129,11 +129,14 @@ class SerialRelay(Node): output = str(self.ser.readline(), "utf8") if output: if output.startswith("can_relay_fromvic,arm,55"): - self.recordAngleFeedback(output) + pass + #self.recordAngleFeedback(output) elif output.startswith("can_relay_fromvic,arm,54"): - self.recordBusVoltage(output) + pass + #self.recordBusVoltage(output) elif output.startswith("can_relay_fromvic,arm,53"): - self.recordMotorFeedback(output) + pass + #self.recordMotorFeedback(output) self.get_logger().info(f"[MCU] {output}") msg = String() msg.data = "From MCU Got: " + output @@ -176,17 +179,17 @@ class SerialRelay(Node): angles.append(0.0)#placeholder for wrist # # - # Update the arm's current angles - self.arm.update_angles(angles) - self.arm_feedback.axis0_angle = angles[0] - self.arm_feedback.axis1_angle = angles[1] - self.arm_feedback.axis2_angle = angles[2] - self.arm_feedback.axis3_angle = angles[3] - self.get_logger().info(f"Angles: {angles}") - #debug publish angles - tempMsg = String() - tempMsg.data = "Angles: " + str(angles) - #self.debug_pub.publish(tempMsg) + # # Update the arm's current angles + # self.arm.update_angles(angles) + # self.arm_feedback.axis0_angle = angles[0] + # self.arm_feedback.axis1_angle = angles[1] + # self.arm_feedback.axis2_angle = angles[2] + # self.arm_feedback.axis3_angle = angles[3] + # self.get_logger().info(f"Angles: {angles}") + # #debug publish angles + # tempMsg = String() + # tempMsg.data = "Angles: " + str(angles) + # #self.debug_pub.publish(tempMsg) else: self.get_logger().info("Invalid angle feedback input format") From 9d13d487cb660cb7b16e8751826c222bde21673b Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 3 May 2025 17:52:49 -0500 Subject: [PATCH 37/49] Fixes for string splitting --- src/arm_pkg/arm_pkg/arm_node.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 866bfc6..0d33617 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -162,7 +162,7 @@ class SerialRelay(Node): def recordAngleFeedback(self, msg): # Angle feedbacks - parts = msg.split(",") + parts = msg.data.split(",") if len(parts) >= 7: # Extract the angles from the string angles_in = parts[3:7] @@ -195,7 +195,7 @@ class SerialRelay(Node): def recordBusVoltage(self, msg): # Bus Voltage feedbacks - parts = msg.split(",") + parts = msg.data.split(",") if len(parts) >= 7: # Extract the voltage from the string voltages_in = parts[3:7] @@ -209,7 +209,7 @@ class SerialRelay(Node): def recordMotorFeedback(self, msg): # Motor voltage/current/temperature feedback - parts = msg.split(",") + parts = msg.data.split(",") if len(parts) >= 7: # Extract the voltage/current/temperature from the string values_in = parts[3:7] From 440a94f0cc86f60ceb0fc801fb5ca4c29a358c11 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sun, 4 May 2025 13:56:14 -0500 Subject: [PATCH 38/49] refactor some things, reenable feedback --- src/arm_pkg/arm_pkg/arm_node.py | 56 ++++++++++++++++++--------------- 1 file changed, 31 insertions(+), 25 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 0d33617..123f19e 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -52,9 +52,11 @@ class SerialRelay(Node): self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, 10) self.man_sub = self.create_subscription(ArmManual, '/arm/control/manual', self.send_manual, 10) + self.ik_debug = self.create_publisher(String, '/arm/debug/ik', 10) + # Topics used in anchor mode if self.launch_mode == 'anchor': - #self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10) + self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10) self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10) @@ -130,13 +132,13 @@ class SerialRelay(Node): if output: if output.startswith("can_relay_fromvic,arm,55"): pass - #self.recordAngleFeedback(output) + #self.updateAngleFeedback(output) elif output.startswith("can_relay_fromvic,arm,54"): pass - #self.recordBusVoltage(output) + #self.updateBusVoltage(output) elif output.startswith("can_relay_fromvic,arm,53"): pass - #self.recordMotorFeedback(output) + #self.updateMotorFeedback(output) self.get_logger().info(f"[MCU] {output}") msg = String() msg.data = "From MCU Got: " + output @@ -160,7 +162,7 @@ class SerialRelay(Node): pass - def recordAngleFeedback(self, msg): + def updateAngleFeedback(self, msg): # Angle feedbacks parts = msg.data.split(",") if len(parts) >= 7: @@ -181,10 +183,10 @@ class SerialRelay(Node): # # # Update the arm's current angles # self.arm.update_angles(angles) - # self.arm_feedback.axis0_angle = angles[0] - # self.arm_feedback.axis1_angle = angles[1] - # self.arm_feedback.axis2_angle = angles[2] - # self.arm_feedback.axis3_angle = angles[3] + self.arm_feedback.axis0_angle = angles[0] + self.arm_feedback.axis1_angle = angles[1] + self.arm_feedback.axis2_angle = angles[2] + self.arm_feedback.axis3_angle = angles[3] # self.get_logger().info(f"Angles: {angles}") # #debug publish angles # tempMsg = String() @@ -193,7 +195,7 @@ class SerialRelay(Node): else: self.get_logger().info("Invalid angle feedback input format") - def recordBusVoltage(self, msg): + def updateBusVoltage(self, msg): # Bus Voltage feedbacks parts = msg.data.split(",") if len(parts) >= 7: @@ -207,7 +209,7 @@ class SerialRelay(Node): else: self.get_logger().info("Invalid voltage feedback input format") - def recordMotorFeedback(self, msg): + def updateMotorFeedback(self, msg): # Motor voltage/current/temperature feedback parts = msg.data.split(",") if len(parts) >= 7: @@ -230,9 +232,9 @@ class SerialRelay(Node): axis2 = msg.axis2 axis3 = msg.axis3 - tempMsg = String() - tempMsg.data = "Sending manual" - self.debug_pub.publish(tempMsg) + # tempMsg = String() + # tempMsg.data = "Sending manual" + # self.debug_pub.publish(tempMsg) #Send controls for arm @@ -380,7 +382,7 @@ class SerialRelay(Node): else: self.get_logger().info("Invalid voltage feedback input format") - def socket_pub_callback(self): + def publish_feedback(self): # Create a SocketFeedback message and publish it # msg = SocketFeedback() # msg.bat_voltage = self.arm_feedback.bat_voltage @@ -394,7 +396,7 @@ class SerialRelay(Node): #debug print self.socket_pub.publish(self.arm_feedback) #Publish feedback from arm - self.arm.update_position() #Run FK and update the current position of the arm, using FK + #self.arm.update_position() #Run FK and update the current position of the arm, using FK @@ -414,12 +416,17 @@ class SerialRelay(Node): target_position = current_position + input_raw + #Print for IK DEBUG + tempMsg = String() + tempMsg.data = "Current Position: " + str(current_position) + "\nInput Vector" + str(input_raw) + "\nTarget Position: " + str(target_position) + self.ik_debug.publish(tempMsg) + # Debug output for current position - tempMsg.data = "Current Position: " + str(current_position) + #tempMsg.data = "Current Position: " + str(current_position) #self.debug_pub.publish(tempMsg) # Debug output for target position - tempMsg.data = "Target Position: " + str(target_position) + #tempMsg.data = "Target Position: " + str(target_position) #self.debug_pub.publish(tempMsg) # Perform IK with the target position @@ -429,20 +436,19 @@ class SerialRelay(Node): self.send_cmd(command) self.get_logger().info(f"IK Success: {target_position}") - tempMsg = String() - tempMsg.data = "IK Success: " + str(target_position) - #self.debug_pub.publish(tempMsg) - tempMsg.data = "Sending: " + str(command) + # tempMsg = String() + # tempMsg.data = "IK Success: " + str(target_position) + # #self.debug_pub.publish(tempMsg) + # tempMsg.data = "Sending: " + str(command) #self.debug_pub.publish(tempMsg) else: self.get_logger().info("IK Fail") - tempMsg = String() - tempMsg.data = "IK Fail" + # tempMsg = String() + # tempMsg.data = "IK Fail" #self.debug_pub.publish(tempMsg) # 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" From 0416277cd901029ec573ff1bd5b9b20cbb27c145 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sun, 4 May 2025 14:09:54 -0500 Subject: [PATCH 39/49] attempting to fix split error --- src/arm_pkg/arm_pkg/arm_node.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 123f19e..cfce36f 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -163,8 +163,11 @@ class SerialRelay(Node): def updateAngleFeedback(self, msg): - # Angle feedbacks - parts = msg.data.split(",") + # Angle feedbacks, + #split the msg.data by commas + msg_str = msg.data + parts = msg_str.split(",") + if len(parts) >= 7: # Extract the angles from the string angles_in = parts[3:7] @@ -197,7 +200,8 @@ class SerialRelay(Node): def updateBusVoltage(self, msg): # Bus Voltage feedbacks - parts = msg.data.split(",") + msg_str = msg.data + parts = msg_str.split(",") if len(parts) >= 7: # Extract the voltage from the string voltages_in = parts[3:7] @@ -211,7 +215,8 @@ class SerialRelay(Node): def updateMotorFeedback(self, msg): # Motor voltage/current/temperature feedback - parts = msg.data.split(",") + msg_str = msg.data + parts = msg_str.split(",") if len(parts) >= 7: # Extract the voltage/current/temperature from the string values_in = parts[3:7] From 3288aea14cdadfbc18b3bd3e64572652d5f40741 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sun, 4 May 2025 14:11:39 -0500 Subject: [PATCH 40/49] revert changes for split --- src/arm_pkg/arm_pkg/arm_node.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index cfce36f..711eeeb 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -165,8 +165,7 @@ class SerialRelay(Node): def updateAngleFeedback(self, msg): # Angle feedbacks, #split the msg.data by commas - msg_str = msg.data - parts = msg_str.split(",") + parts = msg.split(",") if len(parts) >= 7: # Extract the angles from the string @@ -200,8 +199,7 @@ class SerialRelay(Node): def updateBusVoltage(self, msg): # Bus Voltage feedbacks - msg_str = msg.data - parts = msg_str.split(",") + parts = msg.split(",") if len(parts) >= 7: # Extract the voltage from the string voltages_in = parts[3:7] @@ -215,8 +213,7 @@ class SerialRelay(Node): def updateMotorFeedback(self, msg): # Motor voltage/current/temperature feedback - msg_str = msg.data - parts = msg_str.split(",") + parts = msg.split(",") if len(parts) >= 7: # Extract the voltage/current/temperature from the string values_in = parts[3:7] From 2c27c81dc5aedcc59a86c75d495c2eae6d62c54e Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sun, 4 May 2025 14:29:45 -0500 Subject: [PATCH 41/49] update angles for IK when control command is sent --- src/arm_pkg/arm_pkg/arm_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 711eeeb..57fb57b 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -184,7 +184,7 @@ class SerialRelay(Node): # # # # Update the arm's current angles - # self.arm.update_angles(angles) + self.arm.update_angles(angles) self.arm_feedback.axis0_angle = angles[0] self.arm_feedback.axis1_angle = angles[1] self.arm_feedback.axis2_angle = angles[2] From da939cc381e8be98370f17ba2cd5502a6f14d08f Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Wed, 7 May 2025 13:39:48 -0500 Subject: [PATCH 42/49] No publishing controls, to view debug output --- src/arm_pkg/arm_pkg/arm_node.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 57fb57b..96ff8ba 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -131,8 +131,8 @@ class SerialRelay(Node): output = str(self.ser.readline(), "utf8") if output: if output.startswith("can_relay_fromvic,arm,55"): - pass - #self.updateAngleFeedback(output) + #pass + self.updateAngleFeedback(output) elif output.startswith("can_relay_fromvic,arm,54"): pass #self.updateBusVoltage(output) @@ -172,7 +172,7 @@ class SerialRelay(Node): angles_in = parts[3:7] # Convert the angles to floats divide by 10.0 angles = [float(angle) / 10.0 for angle in angles_in] - angles[0] = 0.0 + angles[0] = 0.0 #override axis0 to zero # # #THIS NEEDS TO BE REMOVED LATER @@ -434,7 +434,8 @@ class SerialRelay(Node): # 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" + + #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}") From 2eb3c796ecbdab7c5ad11007f78d16437e6444ca Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Wed, 7 May 2025 13:54:18 -0500 Subject: [PATCH 43/49] Doing testing for performance --- src/arm_pkg/arm_pkg/arm_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 96ff8ba..6689f54 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -131,8 +131,8 @@ class SerialRelay(Node): output = str(self.ser.readline(), "utf8") if output: if output.startswith("can_relay_fromvic,arm,55"): - #pass - self.updateAngleFeedback(output) + pass + #self.updateAngleFeedback(output) elif output.startswith("can_relay_fromvic,arm,54"): pass #self.updateBusVoltage(output) From fb699984b9822a6c74bb2a901943f9a4a39f6c19 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Wed, 7 May 2025 13:56:12 -0500 Subject: [PATCH 44/49] Removing print statements from anchor and core to clean up output --- src/anchor_pkg/anchor_pkg/anchor_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index b21da4a..670eefb 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -88,7 +88,7 @@ class SerialRelay(Node): if output: # All output over debug temporarily - self.get_logger().info(f"[MCU] {output}") + #self.get_logger().info(f"[MCU] {output}") if output.startswith("can_relay_fromvic,arm"): # Publish the message to the arm topic msg = String() From 35208150efba64ec05deb6d378c46692488dada5 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Wed, 7 May 2025 14:02:02 -0500 Subject: [PATCH 45/49] remove command send for perform_ik --- src/arm_pkg/arm_pkg/arm_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 6689f54..c77b347 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -436,7 +436,7 @@ class SerialRelay(Node): # Send command to control #command = "can_relay_tovic,arm,32," + ",".join(map(str, self.arm.ik_angles[:4])) + "\n" - self.send_cmd(command) + #self.send_cmd(command) self.get_logger().info(f"IK Success: {target_position}") # tempMsg = String() From d27023508845ce537ec45928a5462bbeca99f232 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Wed, 7 May 2025 14:07:51 -0500 Subject: [PATCH 46/49] add current angles to ik debug output --- src/arm_pkg/arm_pkg/arm_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index c77b347..31bab2c 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -420,7 +420,7 @@ class SerialRelay(Node): #Print for IK DEBUG tempMsg = String() - tempMsg.data = "Current Position: " + str(current_position) + "\nInput Vector" + str(input_raw) + "\nTarget Position: " + str(target_position) + tempMsg.data = "Current Position: " + str(current_position) + "\nInput Vector" + str(input_raw) + "\nTarget Position: " + str(target_position) + "\nAngles: " + str(self.arm.current_angles) self.ik_debug.publish(tempMsg) # Debug output for current position From bd5c3c3c5abd37f9c5f01775e8040e595361e348 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Wed, 7 May 2025 14:11:24 -0500 Subject: [PATCH 47/49] print debug to screen & ik debug publisher --- src/arm_pkg/arm_pkg/arm_node.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 31bab2c..230e3a1 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -422,6 +422,7 @@ class SerialRelay(Node): tempMsg = String() tempMsg.data = "Current Position: " + str(current_position) + "\nInput Vector" + str(input_raw) + "\nTarget Position: " + str(target_position) + "\nAngles: " + str(self.arm.current_angles) self.ik_debug.publish(tempMsg) + self.get_logger().info(f"[IK] {tempMsg.data}") # Debug output for current position #tempMsg.data = "Current Position: " + str(current_position) From cdc2c7e703d4788e8306eb3df691120b5082f5b5 Mon Sep 17 00:00:00 2001 From: David Date: Thu, 24 Jul 2025 00:07:06 -0500 Subject: [PATCH 48/49] refactor: post-comp IK testing --- src/anchor_pkg/anchor_pkg/anchor_node.py | 33 +-- src/arm_pkg/arm_pkg/arm_node.py | 254 ++++++++--------------- src/arm_pkg/arm_pkg/astra_arm.py | 35 ++-- src/arm_pkg/setup.py | 4 +- src/arm_pkg/urdf/arm12.urdf | 2 +- src/bio_pkg/bio_pkg/bio_node.py | 14 +- src/core_pkg/core_pkg/core_node.py | 26 ++- 7 files changed, 138 insertions(+), 230 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 670eefb..edef967 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -62,9 +62,8 @@ class SerialRelay(Node): if self.port is None: self.get_logger().info("Unable to find MCU...") - #kill the node/process entirely - os.kill(os.getpid(), signal.SIGKILL) - sys.exit(0) + time.sleep(1) + sys.exit(1) self.ser = serial.Serial(self.port, 115200) atexit.register(self.cleanup) @@ -89,23 +88,9 @@ 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) if output.startswith("can_relay_fromvic,core"): self.core_pub.publish(msg) elif output.startswith("can_relay_fromvic,arm") or output.startswith("can_relay_fromvic,digit"): # digit for voltage readings @@ -121,19 +106,19 @@ class SerialRelay(Node): print("Closing serial port.") if self.ser.is_open: self.ser.close() - self.exit(1) + exit(1) except TypeError as e: print(f"TypeError: {e}") print("Closing serial port.") if self.ser.is_open: self.ser.close() - self.exit(1) + exit(1) except Exception as e: print(f"Exception: {e}") - print("Closing serial port.") - if self.ser.is_open: - self.ser.close() - self.exit(1) + # print("Closing serial port.") + # if self.ser.is_open: + # self.ser.close() + # exit(1) def send_cmd(self, msg): message = msg.data diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 230e3a1..9a7c6c0 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -1,5 +1,6 @@ import rclpy from rclpy.node import Node +from rclpy import qos import serial import sys import threading @@ -26,12 +27,21 @@ from ament_index_python.packages import get_package_share_directory from . import astra_arm +control_qos = qos.QoSProfile( + history=qos.QoSHistoryPolicy.KEEP_LAST, + depth=1, + reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, + durability=qos.QoSDurabilityPolicy.VOLATILE, + deadline=1000, + lifespan=500, + liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT, + liveliness_lease_duration=5000 +) + serial_pub = None thread = None - - class SerialRelay(Node): def __init__(self): # Initialize node @@ -49,7 +59,7 @@ class SerialRelay(Node): self.feedback_timer = self.create_timer(0.25, self.publish_feedback) # Create subscribers - self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, 10) + self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, qos_profile=control_qos) self.man_sub = self.create_subscription(ArmManual, '/arm/control/manual', self.send_manual, 10) self.ik_debug = self.create_publisher(String, '/arm/debug/ik', 10) @@ -59,20 +69,11 @@ class SerialRelay(Node): self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10) self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10) - - # output to console - self.get_logger().info("Creating arm object...") - - self.arm_feedback = SocketFeedback() - self.digit_feedback = DigitFeedback() self.arm = astra_arm.Arm('arm12.urdf') self.arm_feedback = SocketFeedback() + self.digit_feedback = DigitFeedback() - ######## - # Interface with MCU # Search for ports IF in 'arm' (standalone) and not 'anchor' mode - ######## - if self.launch_mode == 'arm': # Loop through all serial devices on the computer to check for the MCU self.port = None @@ -84,7 +85,7 @@ class SerialRelay(Node): ser = serial.Serial(port, 115200, timeout=1) #print(f"Checking port {port}...") ser.write(b"ping\n") - response = ser.read_until("\n") + response = ser.read_until("\n") # type: ignore # if pong is in response, then we are talking with the MCU if b"pong" in response: @@ -97,10 +98,9 @@ class SerialRelay(Node): break if self.port is None: - self.get_logger().info("Unable to find MCU...") - #kill the node/process entirely - os.kill(os.getpid(), signal.SIGKILL) - sys.exit(0) + self.get_logger().info("Unable to find MCU... please make sure it is connected.") + time.sleep(1) + sys.exit(1) self.ser = serial.Serial(self.port, 115200) atexit.register(self.cleanup) @@ -130,18 +130,9 @@ class SerialRelay(Node): try: output = str(self.ser.readline(), "utf8") if output: - if output.startswith("can_relay_fromvic,arm,55"): - pass - #self.updateAngleFeedback(output) - elif output.startswith("can_relay_fromvic,arm,54"): - pass - #self.updateBusVoltage(output) - elif output.startswith("can_relay_fromvic,arm,53"): - pass - #self.updateMotorFeedback(output) - self.get_logger().info(f"[MCU] {output}") + #self.get_logger().info(f"[MCU] {output}") msg = String() - msg.data = "From MCU Got: " + output + msg.data = output self.debug_pub.publish(msg) except serial.SerialException: self.get_logger().info("SerialException caught... closing serial port.") @@ -162,9 +153,9 @@ class SerialRelay(Node): pass - def updateAngleFeedback(self, msg): + def updateAngleFeedback(self, msg: str): # Angle feedbacks, - #split the msg.data by commas + # split the msg.data by commas parts = msg.split(",") if len(parts) >= 7: @@ -172,7 +163,7 @@ class SerialRelay(Node): angles_in = parts[3:7] # Convert the angles to floats divide by 10.0 angles = [float(angle) / 10.0 for angle in angles_in] - angles[0] = 0.0 #override axis0 to zero + # angles[0] = 0.0 #override axis0 to zero # # #THIS NEEDS TO BE REMOVED LATER @@ -197,7 +188,8 @@ class SerialRelay(Node): else: self.get_logger().info("Invalid angle feedback input format") - def updateBusVoltage(self, msg): + + def updateBusVoltage(self, msg: str): # Bus Voltage feedbacks parts = msg.split(",") if len(parts) >= 7: @@ -210,23 +202,29 @@ class SerialRelay(Node): self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0 else: self.get_logger().info("Invalid voltage feedback input format") - - def updateMotorFeedback(self, msg): - # Motor voltage/current/temperature feedback - parts = msg.split(",") - if len(parts) >= 7: - # Extract the voltage/current/temperature from the string - values_in = parts[3:7] - # Convert the voltages to floats - for i in range(4): - #update arm_feedback's axisX_temp for each axis0_temp, axis1_temp, etc... - pass - - # self.arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0) - # self.arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0) - # self.arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0) - else: - self.get_logger().info("Invalid motor feedback input format") + + def updateMotorFeedback(self, msg: str): + parts = str(msg.strip()).split(",") + motorId = round(float(parts[3])) + temp = float(parts[4]) / 10.0 + voltage = float(parts[5]) / 10.0 + current = float(parts[6]) / 10.0 + if motorId == 1: + self.arm_feedback.axis1_temp = temp + self.arm_feedback.axis1_voltage = voltage + self.arm_feedback.axis1_current = current + elif motorId == 2: + self.arm_feedback.axis2_temp = temp + self.arm_feedback.axis2_voltage = voltage + self.arm_feedback.axis2_current = current + elif motorId == 3: + self.arm_feedback.axis3_temp = temp + self.arm_feedback.axis3_voltage = voltage + self.arm_feedback.axis3_current = current + elif motorId == 4: + self.arm_feedback.axis0_temp = temp + self.arm_feedback.axis0_voltage = voltage + self.arm_feedback.axis0_current = current def send_manual(self, msg: ArmManual): axis0 = msg.axis0 @@ -234,11 +232,6 @@ class SerialRelay(Node): axis2 = msg.axis2 axis3 = msg.axis3 - # tempMsg = String() - # tempMsg.data = "Sending manual" - # self.debug_pub.publish(tempMsg) - - #Send controls for arm command = "can_relay_tovic,arm,18," + str(int(msg.brake)) + "\n" command += "can_relay_tovic,arm,39," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n" @@ -293,27 +286,7 @@ class SerialRelay(Node): #pass self.updateBusVoltage(output) elif output.startswith("can_relay_fromvic,arm,53"): - parts = str(output.strip()).split(",") - motorId = round(float(parts[3])) - temp = float(parts[4]) / 10.0 - voltage = float(parts[5]) / 10.0 - current = float(parts[6]) / 10.0 - if motorId == 1: - self.arm_feedback.axis1_temp = temp - self.arm_feedback.axis1_voltage = voltage - self.arm_feedback.axis1_current = current - elif motorId == 2: - self.arm_feedback.axis2_temp = temp - self.arm_feedback.axis2_voltage = voltage - self.arm_feedback.axis2_current = current - elif motorId == 3: - self.arm_feedback.axis3_temp = temp - self.arm_feedback.axis3_voltage = voltage - self.arm_feedback.axis3_current = current - elif motorId == 4: - self.arm_feedback.axis0_temp = temp - self.arm_feedback.axis0_voltage = voltage - self.arm_feedback.axis0_current = current + self.updateMotorFeedback(output) elif output.startswith("can_relay_fromvic,digit,54"): parts = msg.data.split(",") if len(parts) >= 7: @@ -334,80 +307,18 @@ class SerialRelay(Node): self.socket_pub.publish(self.arm_feedback) self.digit_pub.publish(self.digit_feedback) - def updateAngleFeedback(self, msg: str): - # Angle feedbacks, - #split the msg.data by commas - parts = msg.split(",") - - if len(parts) >= 7: - # Extract the angles from the string - angles_in = parts[3:7] - # Convert the angles to floats divide by 10.0 - angles = [float(angle) / 10.0 for angle in angles_in] - #angles[0] = 0.0 #override axis0 to zero - # - # - #THIS NEEDS TO BE REMOVED LATER - #PLACEHOLDER FOR WRIST VALUE - # - # - angles.append(0.0)#placeholder for wrist_continuous - angles.append(0.0)#placeholder for wrist - # - # - # # Update the arm's current angles - #self.arm.update_angles(angles) - self.arm_feedback.axis0_angle = angles[0] - self.arm_feedback.axis1_angle = angles[1] - self.arm_feedback.axis2_angle = angles[2] - self.arm_feedback.axis3_angle = angles[3] - # self.get_logger().info(f"Angles: {angles}") - # #debug publish angles - # tempMsg = String() - # tempMsg.data = "Angles: " + str(angles) - # #self.debug_pub.publish(tempMsg) - else: - self.get_logger().info("Invalid angle feedback input format") - - - def updateBusVoltage(self, msg: str): - # Bus Voltage feedbacks - parts = msg.split(",") - if len(parts) >= 7: - # 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 - else: - self.get_logger().info("Invalid voltage feedback input format") - - def publish_feedback(self): - # Create a SocketFeedback message and publish it - # msg = SocketFeedback() - # msg.bat_voltage = self.arm_feedback.bat_voltage - # msg.voltage_12 = self.arm_feedback.voltage_12 - # msg.voltage_5 = self.arm_feedback.voltage_5 - # msg.voltage_3 = self.arm_feedback.voltage_3 - # msg.joint_angles = self.arm_feedback.joint_angles - # msg.joint_temps = self.arm_feedback.joint_temps - # msg.joint_voltages = self.arm_feedback.joint_voltages - # msg.joint_currents = self.arm_feedback.joint_currents - #debug print - self.socket_pub.publish(self.arm_feedback) #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): # Convert Vector3 to a NumPy array - input_raw = np.array([msg.movement_vector.x, msg.movement_vector.y, msg.movement_vector.z]) # Convert input to a NumPy array + input_raw = np.array([-msg.movement_vector.x, msg.movement_vector.y, msg.movement_vector.z]) # Convert input to a NumPy array # decrease input vector by 90% input_raw = input_raw * 0.2 + if input_raw[0] == 0.0 and input_raw[1] == 0.0 and input_raw[2] == 0.0: + self.get_logger().info("No input, stopping arm.") + command = "can_relay_tovic,arm,39,0,0,0,0\n" + self.send_cmd(command) + return + # Debug output tempMsg = String() tempMsg.data = "From IK Control Got Vector: " + str(input_raw) @@ -420,7 +331,8 @@ class SerialRelay(Node): #Print for IK DEBUG tempMsg = String() - tempMsg.data = "Current Position: " + str(current_position) + "\nInput Vector" + str(input_raw) + "\nTarget Position: " + str(target_position) + "\nAngles: " + str(self.arm.current_angles) + # tempMsg.data = "Current Position: " + str(current_position) + "\nInput Vector" + str(input_raw) + "\nTarget Position: " + str(target_position) + "\nAngles: " + str(self.arm.current_angles) + tempMsg.data = "Current Angles: " + str(math.degrees(self.arm.current_angles[2])) + ", " + str(math.degrees(self.arm.current_angles[4])) + ", " + str(math.degrees(self.arm.current_angles[6])) self.ik_debug.publish(tempMsg) self.get_logger().info(f"[IK] {tempMsg.data}") @@ -433,48 +345,41 @@ class SerialRelay(Node): #self.debug_pub.publish(tempMsg) # Perform IK with the target position - if self.arm.perform_ik(target_position): + if self.arm.perform_ik(target_position, self.get_logger()): # 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}") + self.get_logger().info(f"IK Angles: [{str(round(math.degrees(self.arm.ik_angles[2]), 2))}, {str(round(math.degrees(self.arm.ik_angles[4]), 2))}, {str(round(math.degrees(self.arm.ik_angles[6]), 2))}]") # tempMsg = String() # tempMsg.data = "IK Success: " + str(target_position) # #self.debug_pub.publish(tempMsg) # tempMsg.data = "Sending: " + str(command) #self.debug_pub.publish(tempMsg) + + # Send the IK angles to the MCU + command = "can_relay_tovic,arm,32," + f"{math.degrees(self.arm.ik_angles[0])*10},{math.degrees(self.arm.ik_angles[2])*10},{math.degrees(self.arm.ik_angles[4])*10},{math.degrees(self.arm.ik_angles[6])*10}" + "\n" + # self.send_cmd(command) + + # Manual control for Wrist/Effector + command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n" + + command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n" + + command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n" + + command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n" + + self.send_cmd(command) else: self.get_logger().info("IK Fail") + self.get_logger().info(f"IK Angles: [{str(math.degrees(self.arm.ik_angles[2]))}, {str(math.degrees(self.arm.ik_angles[4]))}, {str(math.degrees(self.arm.ik_angles[6]))}]") # tempMsg = String() # tempMsg.data = "IK Fail" #self.debug_pub.publish(tempMsg) - # 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 - - - - @staticmethod def list_serial_ports(): @@ -483,8 +388,11 @@ class SerialRelay(Node): def cleanup(self): print("Cleaning up...") - if self.ser.is_open: - self.ser.close() + try: + if self.ser.is_open: + self.ser.close() + except Exception as e: + exit(0) def myexcepthook(type, value, tb): print("Uncaught exception:", type, value) diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py index a61bd3a..44a1373 100644 --- a/src/arm_pkg/arm_pkg/astra_arm.py +++ b/src/arm_pkg/arm_pkg/astra_arm.py @@ -1,3 +1,5 @@ +import rclpy +from rclpy.node import Node import numpy as np import time, math, os from math import sin, cos, pi @@ -15,16 +17,16 @@ 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] + return [0.0, math.radians(angles[0]), math.radians(angles[1]), 0.0, math.radians(angles[2]), 0.0, math.radians(angles[3]), 0.0, math.radians(angles[4]), math.radians(angles[5]), 0.0] class Arm: def __init__(self, urdf_name): - self.ik_tolerance = 1e-3 #Tolerance (in meters) to determine if solution is valid + self.ik_tolerance = 1e-1 #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) + self.urdf = os.path.join(get_package_share_directory('arm_pkg'), 'urdf', urdf_name) # IKpy Chain - self.chain = Chain.from_urdf_file(self.urdf) + self.chain = Chain.from_urdf_file(self.urdf) # Arrays for joint states @@ -35,16 +37,16 @@ class Arm: self.last_angles = self.zero_angles self.ik_angles = self.zero_angles - self.current_position = [] + self.current_position: list[float] = [] self.target_position = [0.0, 0.0, 0.0] - self.target_orientation = [] # Effector orientation desired at target position. + self.target_orientation: list = [] # 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): + def perform_ik(self, target_position, logger): self.target_position = target_position # Update the target orientation to the current orientation self.update_orientation() @@ -62,19 +64,19 @@ class Arm: # Check if the solution is within the tolerance fk_matrix = self.chain.forward_kinematics(self.ik_angles) - fk_position = fk_matrix[:3, 3] + fk_position = fk_matrix[:3, 3] # type: ignore # 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}") + logger.info(f"No VALID IK Solution within tolerance. Error: {error}") return False else: - print(f"IK Solution Found. Error: {error}") + logger.info(f"IK Solution Found. Error: {error}") return True except Exception as e: - print(f"IK failed for exception: {e}") + logger.info(f"IK failed for exception: {e}") return False # # Given the FK_Matix for the arm's current pose, update the orientation array @@ -93,7 +95,7 @@ class Arm: 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] + self.target_orientation = fk_matrix[:3, :3] # type: ignore # Update current angles to those provided # Resetting last_angles to the new angles @@ -119,7 +121,7 @@ class Arm: 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] + position = fk_matrix[:3, 3] # type: ignore return position @@ -129,7 +131,7 @@ class Arm: 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] + position = fk_matrix[:3, 3] # type: ignore # Return position as a NumPy array return np.array(position) @@ -140,7 +142,4 @@ class Arm: 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] - - - + self.current_position = fk_matrix[:3, 3] # type: ignore diff --git a/src/arm_pkg/setup.py b/src/arm_pkg/setup.py index 6d55985..089982a 100644 --- a/src/arm_pkg/setup.py +++ b/src/arm_pkg/setup.py @@ -12,8 +12,8 @@ setup( ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), - (os.path.join('share', package_name), glob('urdf/*')), + (os.path.join('share', package_name, 'launch'), glob('launch/*')), + (os.path.join('share', package_name, 'urdf'), glob('urdf/*')), ], install_requires=['setuptools'], zip_safe=True, diff --git a/src/arm_pkg/urdf/arm12.urdf b/src/arm_pkg/urdf/arm12.urdf index 6adb724..b63d872 100644 --- a/src/arm_pkg/urdf/arm12.urdf +++ b/src/arm_pkg/urdf/arm12.urdf @@ -115,7 +115,7 @@ - + diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index 5d6fa7a..94b4fba 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -66,10 +66,9 @@ class SerialRelay(Node): pass if self.port is None: - self.get_logger().info("Unable to find MCU...") - #kill the node/process entirely - os.kill(os.getpid(), signal.SIGKILL) - sys.exit(0) + self.get_logger().info("Unable to find MCU... please make sure it is connected.") + time.sleep(1) + sys.exit(1) self.ser = serial.Serial(self.port, 115200) atexit.register(self.cleanup) @@ -204,8 +203,11 @@ class SerialRelay(Node): def cleanup(self): print("Cleaning up...") - if self.ser.is_open: - self.ser.close() + try: + if self.ser.is_open: + self.ser.close() + except Exception as e: + exit(0) def myexcepthook(type, value, tb): print("Uncaught exception:", type, value) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index ac50a69..9004705 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -1,5 +1,6 @@ import rclpy from rclpy.node import Node +from rclpy import qos from std_srvs.srv import Empty import signal @@ -19,6 +20,17 @@ from ros2_interfaces_pkg.msg import CoreControl serial_pub = None thread = None +control_qos = qos.QoSProfile( + history=qos.QoSHistoryPolicy.KEEP_LAST, + depth=1, + reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, + durability=qos.QoSDurabilityPolicy.VOLATILE, + deadline=1000, + lifespan=500, + liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT, + liveliness_lease_duration=5000 +) + class SerialRelay(Node): def __init__(self): # Initalize node with name @@ -33,7 +45,7 @@ class SerialRelay(Node): self.debug_pub = self.create_publisher(String, '/core/debug', 10) self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10) # Create a subscriber - self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10) + self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, qos_profile=control_qos) # Create a publisher for telemetry self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback) @@ -75,9 +87,8 @@ class SerialRelay(Node): if self.port is None: self.get_logger().info("Unable to find MCU...") - #kill the node/process entirely - os.kill(os.getpid(), signal.SIGKILL) - sys.exit(0) + time.sleep(1) + sys.exit(1) self.ser = serial.Serial(self.port, 115200) atexit.register(self.cleanup) @@ -265,8 +276,11 @@ class SerialRelay(Node): def cleanup(self): print("Cleaning up before terminating...") - if self.ser.is_open: - self.ser.close() + try: + if self.ser.is_open: + self.ser.close() + except Exception as e: + exit(0) def myexcepthook(type, value, tb): print("Uncaught exception:", type, value) From 4e1e0e29dd5b410154dd17fde48789908b3a1f56 Mon Sep 17 00:00:00 2001 From: David Date: Tue, 12 Aug 2025 08:49:12 -0500 Subject: [PATCH 49/49] style: get ready for main --- src/arm_pkg/arm_pkg/arm_node.py | 317 ++++++++++++++--------------- src/core_pkg/core_pkg/core_node.py | 22 +- 2 files changed, 162 insertions(+), 177 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 9a7c6c0..6b838d2 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -27,16 +27,16 @@ from ament_index_python.packages import get_package_share_directory from . import astra_arm -control_qos = qos.QoSProfile( - history=qos.QoSHistoryPolicy.KEEP_LAST, - depth=1, - reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, - durability=qos.QoSDurabilityPolicy.VOLATILE, - deadline=1000, - lifespan=500, - liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT, - liveliness_lease_duration=5000 -) +# control_qos = qos.QoSProfile( +# history=qos.QoSHistoryPolicy.KEEP_LAST, +# depth=1, +# reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, +# durability=qos.QoSDurabilityPolicy.VOLATILE, +# deadline=1000, +# lifespan=500, +# liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT, +# liveliness_lease_duration=5000 +# ) serial_pub = None thread = None @@ -59,7 +59,7 @@ class SerialRelay(Node): self.feedback_timer = self.create_timer(0.25, self.publish_feedback) # Create subscribers - self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, qos_profile=control_qos) + self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, 10) self.man_sub = self.create_subscription(ArmManual, '/arm/control/manual', self.send_manual, 10) self.ik_debug = self.create_publisher(String, '/arm/debug/ik', 10) @@ -152,161 +152,6 @@ class SerialRelay(Node): self.ser.close() pass - - def updateAngleFeedback(self, msg: str): - # Angle feedbacks, - # split the msg.data by commas - parts = msg.split(",") - - if len(parts) >= 7: - # Extract the angles from the string - angles_in = parts[3:7] - # Convert the angles to floats divide by 10.0 - angles = [float(angle) / 10.0 for angle in angles_in] - # angles[0] = 0.0 #override axis0 to zero - # - # - #THIS NEEDS TO BE REMOVED LATER - #PLACEHOLDER FOR WRIST VALUE - # - # - angles.append(0.0)#placeholder for wrist_continuous - angles.append(0.0)#placeholder for wrist - # - # - # # Update the arm's current angles - self.arm.update_angles(angles) - self.arm_feedback.axis0_angle = angles[0] - self.arm_feedback.axis1_angle = angles[1] - self.arm_feedback.axis2_angle = angles[2] - self.arm_feedback.axis3_angle = angles[3] - # self.get_logger().info(f"Angles: {angles}") - # #debug publish angles - # tempMsg = String() - # tempMsg.data = "Angles: " + str(angles) - # #self.debug_pub.publish(tempMsg) - else: - self.get_logger().info("Invalid angle feedback input format") - - - def updateBusVoltage(self, msg: str): - # Bus Voltage feedbacks - parts = msg.split(",") - if len(parts) >= 7: - # 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 - else: - self.get_logger().info("Invalid voltage feedback input format") - - def updateMotorFeedback(self, msg: str): - parts = str(msg.strip()).split(",") - motorId = round(float(parts[3])) - temp = float(parts[4]) / 10.0 - voltage = float(parts[5]) / 10.0 - current = float(parts[6]) / 10.0 - if motorId == 1: - self.arm_feedback.axis1_temp = temp - self.arm_feedback.axis1_voltage = voltage - self.arm_feedback.axis1_current = current - elif motorId == 2: - self.arm_feedback.axis2_temp = temp - self.arm_feedback.axis2_voltage = voltage - self.arm_feedback.axis2_current = current - elif motorId == 3: - self.arm_feedback.axis3_temp = temp - self.arm_feedback.axis3_voltage = voltage - self.arm_feedback.axis3_current = current - elif motorId == 4: - self.arm_feedback.axis0_temp = temp - self.arm_feedback.axis0_voltage = voltage - self.arm_feedback.axis0_current = current - - def send_manual(self, msg: ArmManual): - axis0 = msg.axis0 - axis1 = -1 * msg.axis1 - axis2 = msg.axis2 - axis3 = msg.axis3 - - #Send controls for arm - command = "can_relay_tovic,arm,18," + str(int(msg.brake)) + "\n" - command += "can_relay_tovic,arm,39," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n" - - #Send controls for end effector - command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n" - - - command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n" - - - command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n" - - - command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n" - - command += "can_relay_tovic,digit,34," + str(msg.linear_actuator) + "\n" - - self.send_cmd(command) - - - - #print(f"[Wrote] {command}", end="") - - #Not yet finished, needs embedded implementation for new commands - # ef_roll = msg.effector_roll - # ef_yaw = msg.effector_yaw - # gripper = msg.gripper - # actuator = msg.linear_actuator - # laser = msg.laser - # #Send controls for digit - - # command = "can_relay_tovic,digit," + str(ef_roll) + "," + str(ef_yaw) + "," + str(gripper) + "," + str(actuator) + "," + str(laser) + "\n" - - return - - def send_cmd(self, msg: str): - if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay - output = String() - output.data = msg - self.anchor_pub.publish(output) - elif self.launch_mode == 'arm': #if in standalone mode, send to MCU directly - self.get_logger().info(f"[Arm to MCU] {msg.data}") - self.ser.write(bytes(msg, "utf8")) - - def anchor_feedback(self, msg: String): - output = msg.data - if output.startswith("can_relay_fromvic,arm,55"): - #pass - self.updateAngleFeedback(output) - elif output.startswith("can_relay_fromvic,arm,54"): - #pass - self.updateBusVoltage(output) - elif output.startswith("can_relay_fromvic,arm,53"): - self.updateMotorFeedback(output) - elif output.startswith("can_relay_fromvic,digit,54"): - parts = msg.data.split(",") - if len(parts) >= 7: - # Extract the voltage from the string - voltages_in = parts[3:7] - # Convert the voltages to floats - self.digit_feedback.bat_voltage = float(voltages_in[0]) / 100.0 - self.digit_feedback.voltage_12 = float(voltages_in[1]) / 100.0 - self.digit_feedback.voltage_5 = float(voltages_in[2]) / 100.0 - elif output.startswith("can_relay_fromvic,digit,55"): - parts = msg.data.split(",") - if len(parts) >= 4: - self.digit_feedback.wrist_angle = float(parts[3]) - else: - return - - def publish_feedback(self): - self.socket_pub.publish(self.arm_feedback) - self.digit_pub.publish(self.digit_feedback) - def send_ik(self, msg): # Convert Vector3 to a NumPy array input_raw = np.array([-msg.movement_vector.x, msg.movement_vector.y, msg.movement_vector.z]) # Convert input to a NumPy array @@ -381,6 +226,146 @@ class SerialRelay(Node): #self.debug_pub.publish(tempMsg) + def send_manual(self, msg: ArmManual): + axis0 = msg.axis0 + axis1 = -1 * msg.axis1 + axis2 = msg.axis2 + axis3 = msg.axis3 + + #Send controls for arm + command = "can_relay_tovic,arm,18," + str(int(msg.brake)) + "\n" + command += "can_relay_tovic,arm,39," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n" + + #Send controls for end effector + command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n" + + + command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n" + + + command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n" + + + command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n" + + command += "can_relay_tovic,digit,34," + str(msg.linear_actuator) + "\n" + + self.send_cmd(command) + + return + + def send_cmd(self, msg: str): + if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay + output = String() + output.data = msg + self.anchor_pub.publish(output) + elif self.launch_mode == 'arm': #if in standalone mode, send to MCU directly + self.get_logger().info(f"[Arm to MCU] {msg.data}") + self.ser.write(bytes(msg, "utf8")) + + def anchor_feedback(self, msg: String): + output = msg.data + if output.startswith("can_relay_fromvic,arm,55"): + #pass + self.updateAngleFeedback(output) + elif output.startswith("can_relay_fromvic,arm,54"): + #pass + self.updateBusVoltage(output) + elif output.startswith("can_relay_fromvic,arm,53"): + self.updateMotorFeedback(output) + elif output.startswith("can_relay_fromvic,digit,54"): + parts = msg.data.split(",") + if len(parts) >= 7: + # Extract the voltage from the string + voltages_in = parts[3:7] + # Convert the voltages to floats + self.digit_feedback.bat_voltage = float(voltages_in[0]) / 100.0 + self.digit_feedback.voltage_12 = float(voltages_in[1]) / 100.0 + self.digit_feedback.voltage_5 = float(voltages_in[2]) / 100.0 + elif output.startswith("can_relay_fromvic,digit,55"): + parts = msg.data.split(",") + if len(parts) >= 4: + self.digit_feedback.wrist_angle = float(parts[3]) + else: + return + + def publish_feedback(self): + self.socket_pub.publish(self.arm_feedback) + self.digit_pub.publish(self.digit_feedback) + + def updateAngleFeedback(self, msg: str): + # Angle feedbacks, + # split the msg.data by commas + parts = msg.split(",") + + if len(parts) >= 7: + # Extract the angles from the string + angles_in = parts[3:7] + # Convert the angles to floats divide by 10.0 + angles = [float(angle) / 10.0 for angle in angles_in] + # angles[0] = 0.0 #override axis0 to zero + # + # + #THIS NEEDS TO BE REMOVED LATER + #PLACEHOLDER FOR WRIST VALUE + # + # + angles.append(0.0)#placeholder for wrist_continuous + angles.append(0.0)#placeholder for wrist + # + # + # # Update the arm's current angles + self.arm.update_angles(angles) + self.arm_feedback.axis0_angle = angles[0] + self.arm_feedback.axis1_angle = angles[1] + self.arm_feedback.axis2_angle = angles[2] + self.arm_feedback.axis3_angle = angles[3] + # self.get_logger().info(f"Angles: {angles}") + # #debug publish angles + # tempMsg = String() + # tempMsg.data = "Angles: " + str(angles) + # #self.debug_pub.publish(tempMsg) + else: + self.get_logger().info("Invalid angle feedback input format") + + def updateBusVoltage(self, msg: str): + # Bus Voltage feedbacks + parts = msg.split(",") + if len(parts) >= 7: + # 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 + else: + self.get_logger().info("Invalid voltage feedback input format") + + def updateMotorFeedback(self, msg: str): + parts = str(msg.strip()).split(",") + motorId = round(float(parts[3])) + temp = float(parts[4]) / 10.0 + voltage = float(parts[5]) / 10.0 + current = float(parts[6]) / 10.0 + if motorId == 1: + self.arm_feedback.axis1_temp = temp + self.arm_feedback.axis1_voltage = voltage + self.arm_feedback.axis1_current = current + elif motorId == 2: + self.arm_feedback.axis2_temp = temp + self.arm_feedback.axis2_voltage = voltage + self.arm_feedback.axis2_current = current + elif motorId == 3: + self.arm_feedback.axis3_temp = temp + self.arm_feedback.axis3_voltage = voltage + self.arm_feedback.axis3_current = current + elif motorId == 4: + self.arm_feedback.axis0_temp = temp + self.arm_feedback.axis0_voltage = voltage + self.arm_feedback.axis0_current = current + + @staticmethod def list_serial_ports(): return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*") diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 9004705..6772e28 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -20,16 +20,16 @@ from ros2_interfaces_pkg.msg import CoreControl serial_pub = None thread = None -control_qos = qos.QoSProfile( - history=qos.QoSHistoryPolicy.KEEP_LAST, - depth=1, - reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, - durability=qos.QoSDurabilityPolicy.VOLATILE, - deadline=1000, - lifespan=500, - liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT, - liveliness_lease_duration=5000 -) +# control_qos = qos.QoSProfile( +# history=qos.QoSHistoryPolicy.KEEP_LAST, +# depth=1, +# reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, +# durability=qos.QoSDurabilityPolicy.VOLATILE, +# deadline=1000, +# lifespan=500, +# liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT, +# liveliness_lease_duration=5000 +# ) class SerialRelay(Node): def __init__(self): @@ -45,7 +45,7 @@ class SerialRelay(Node): self.debug_pub = self.create_publisher(String, '/core/debug', 10) self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10) # Create a subscriber - self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, qos_profile=control_qos) + self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10) # Create a publisher for telemetry self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)