aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 09:20:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

swap to numpy arrays

Browse Source
This commit is contained in:
Tristan McGinnis
2025-04-26 12:17:10 -05:00
committed by David
parent 3d18e20946
commit 60952db588
2 changed files with 11 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
src/arm_pkg/arm_pkg/arm_node.py
View File

@@ -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)

9
src/arm_pkg/arm_pkg/astra_arm.py
View File

@@ -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):