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

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

@@ -48,7 +48,11 @@ class Arm:
self.target_position = target_position
# Update the target orientation to the current orientation
self.update_orientation()
print(f"[IK FOR] Target Position: {self.target_position}")
try:
print(f"[TRY] Current Angles: {self.current_angles}")
print(f"[TRY] Target Position: {self.target_position}")
print(f"[TRY] Target Orientation: {self.target_orientation}")
self.ik_angles = self.chain.inverse_kinematics(
target_position=self.target_position,
target_orientation=self.target_orientation,
@@ -57,18 +61,31 @@ class Arm:
)
# Check if the solution is within the tolerance
fk_matrix = self.chain.forward_kinematics(self.ik_angles)
fk_position = fk_matrix[:3, 3]
print(f"[TRY] FK Position for Solution: {fk_position}")
error = np.linalg.norm(target_position - fk_position)
if error > self.ik_tolerance:
self.get_logger().info(f"No VALID IK Solution within tolerance. Error: {error}")
print(f"No VALID IK Solution within tolerance. Error: {error}")
return False
else:
self.get_logger().info(f"IK Solution Found. Error: {error}")
print(f"IK Solution Found. Error: {error}")
return True
except Exception as e:
print(f"IK failed: {e}")
print(f"IK failed for exception: {e}")
return False
# # 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
# Get current orientation of the end effector and update target_orientation
def update_orientation(self):