From 0adab485f2add6482b99be81ea50a7c05c871044 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 22 Mar 2025 11:17:18 -0500 Subject: [PATCH] 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