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): def send_ik(self, msg):
input_raw = msg.movement_vector # [x, y, z] input_raw = np.array(msg.movement_vector) # Convert input to a NumPy array
#input_x = input_raw[0]
#input_y = input_raw[1]
#input_z = input_raw[2]
# Debug output # Debug output
tempMsg = String() tempMsg = String()
tempMsg.data = "From IK Control Got Vector: " + str(input_raw) tempMsg.data = "From IK Control Got Vector: " + str(input_raw)
#self.get_logger().info(f"[IK Control] {tempMsg.data}")
self.debug_pub.publish(tempMsg) self.debug_pub.publish(tempMsg)
# normalize the vector # Target position is current position + input vector
#input_norm = np.linalg.norm(input_raw) / 2.0 current_position = self.arm.get_position_vector()
target_position = current_position + input_raw
# Debug output for target position
#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))
tempMsg.data = "Target Position: " + str(target_position) tempMsg.data = "Target Position: " + str(target_position)
self.debug_pub.publish(tempMsg) self.debug_pub.publish(tempMsg)
if(self.arm.perform_ik(self.arm.get_position()+input_norm)): # Perform IK with the target position
#send command to control if self.arm.perform_ik(target_position):
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" # 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}") self.get_logger().info(f"IK Success: {target_position}")
@@ -429,14 +422,12 @@ class SerialRelay(Node):
self.debug_pub.publish(msg) self.debug_pub.publish(msg)
msg.data = "Sending: " + str(command) msg.data = "Sending: " + str(command)
self.debug_pub.publish(msg) self.debug_pub.publish(msg)
else: else:
self.get_logger().info("IK Fail") self.get_logger().info("IK Fail")
msg = String() msg = String()
msg.data = "IK Fail" msg.data = "IK Fail"
self.debug_pub.publish(msg) self.debug_pub.publish(msg)
# Manual control for Wrist/Effector # Manual control for Wrist/Effector
command = "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n" command = "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
self.send_cmd(command) 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 # Get the position of the end effector from the FK matrix
position = fk_matrix[:3, 3] position = fk_matrix[:3, 3]
# Convert position to Vector3 # Return position as a NumPy array
position_vector = Vector3() return np.array(position)
position_vector.x = position[0]
position_vector.y = position[1]
position_vector.z = position[2]
return position_vector
def update_position(self): def update_position(self):