From 5eb9e8a2e3e76fc4e7bb2fe45c1ef92f2fa73c25 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 26 Apr 2025 12:08:46 -0500 Subject: [PATCH] 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 +