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

fixes and debug to ik- need to update the model

Browse Source
This commit is contained in:
ASTRA-SHC
2025-04-26 13:04:20 -05:00
committed by David
parent 60952db588
commit 2860463501
2 changed files with 42 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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