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

Revert "add anchor relay to feedback topics for bio,arm,core"

Browse Source 
This reverts commit d38a84abca.
This commit is contained in:
Tristan McGinnis
2025-05-03 18:03:12 -05:00
parent d38a84abca
commit 732cb8c1b5
3 changed files with 4 additions and 156 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -206,10 +206,10 @@ class SerialRelay(Node):
# Extract the voltage from the string
voltages_in = parts[3:7]
# Convert the voltages to floats
# self.arm_feedback.bat_voltage = float(voltages_in[0]) / 100.0
# self.arm_feedback.voltage_12 = float(voltages_in[1]) / 100.0
# self.arm_feedback.voltage_5 = float(voltages_in[2]) / 100.0
# self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0
self.arm_feedback.bat_voltage = float(voltages_in[0]) / 100.0
self.arm_feedback.voltage_12 = float(voltages_in[1]) / 100.0
self.arm_feedback.voltage_5 = float(voltages_in[2]) / 100.0
self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0
else:
self.get_logger().info("Invalid voltage feedback input format")

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

@@ -1,137 +0,0 @@
import numpy as np
import time, math, os
from math import sin, cos, pi
from ament_index_python.packages import get_package_share_directory
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
degree = pi / 180.0
def convert_angles(angles):
# Converts angles to the format used for the urdf (contains some dummy joints)
return [0.0, angles[0]*degree, angles[1]*degree, 0.0, angles[2]*degree, 0.0, angles[3]*degree, 0.0, angles[4]*degree, angles[5]*degree, 0.0]
class Arm:
def __init__(self, urdf_name):
self.ik_tolerance = 1e-3 #Tolerance (in meters) to determine if solution is valid
# URDF file path
self.urdf = os.path.join(get_package_share_directory('arm_pkg'), urdf_name)
# IKpy Chain
self.chain = Chain.from_urdf_file(self.urdf)
# Arrays for joint states
# Some links in the URDF are static (non-joints), these will remain zero for IK
# Indexes: Fixed_base, Ax_0, Ax_1, seg1, Ax_2, seg2, ax_3, seg3, continuous, wrist, Effector
self.zero_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.current_angles = self.zero_angles
self.last_angles = self.zero_angles
self.ik_angles = self.zero_angles
self.current_position = []
self.target_position = [0.0, 0.0, 0.0]
self.target_orientation = [] # Effector orientation desired at target position.
# Generally orientation for the effector is modified manually by the operator.
# Might not need, copied over from state_publisher.py in ik_test
#self.step = 0.03 # Max movement increment
def perform_ik(self, target_position):
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,
initial_position=self.current_angles,
orientation_mode="all"
)
# 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:
print(f"No VALID IK Solution within tolerance. Error: {error}")
return False
else:
print(f"IK Solution Found. Error: {error}")
return True
except Exception as e:
print(f"IK failed for exception: {e}")
return False
# Get current orientation of the end effector and update target_orientation
def update_orientation(self):
# FK matrix for arm's current pose
fk_matrix = self.chain.forward_kinematics(self.current_angles)
# Update target_orientation to the effector's current orientation
self.target_orientation = fk_matrix[:3, :3]
# Update current angles to those provided
# Resetting last_angles to the new angles
#
# Use: First call, or when angles are changed manually.
def reset_angles(self, angles):
# Update angles to the new angles
self.current_angles = convert_angles(angles)
self.last_angles = self.current_angles
# Update current angles to those provided
# Maintain previous angles in last_angles
#
# Use: Repeated calls during IK operation
def update_angles(self, angles):
# Update angles to the new angles
self.last_angles = self.current_angles
self.current_angles = convert_angles(angles)
# Get current X,Y,Z position of end effector
def get_position(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]
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]
# Return position as a NumPy array
return np.array(position)
def update_position(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 and update current pos
self.current_position = fk_matrix[:3, 3]