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

Convert angles to radians for IK

Browse Source
This commit is contained in:
Tristan McGinnis
2025-05-03 12:43:17 -05:00
committed by David
parent 2860463501
commit c9533e3f55
3 changed files with 316 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -63,7 +63,7 @@ class SerialRelay(Node):
self.arm_feedback = SocketFeedback()
self.digit_feedback = DigitFeedback()
self.arm = astra_arm.Arm('arm11.urdf')
self.arm = astra_arm.Arm('arm12.urdf')
self.arm_feedback = SocketFeedback()
########
@@ -168,22 +168,13 @@ class SerialRelay(Node):
angles[0] = 0.0
#
#
#
#THIS NEEDS TO BE REMOVED LATER
#PLACEHOLDER FOR WRIST VALUE
#
##
#
#
#
angles.append(0.0)
#
#
#
#
#
##
#
# Update the arm's current angles
self.arm.update_angles(angles)
self.arm_feedback.axis0_angle = angles[0]

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

@@ -15,7 +15,7 @@ degree = pi / 180.0
def convert_angles(angles):
# Converts angles to the format used for the urdf (contains some dummy joints)
return [0.0, 0.0, angles[0], angles[1], angles[2], angles[3], angles[4], 0.0]
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]
class Arm:
@@ -29,8 +29,8 @@ class Arm:
# Arrays for joint states
# Some links in the URDF are static (non-joints), these will remain zero for IK
# Indexes: Ignore, Ignore, Ax_0, Ax_1, Ax2, Ax_3, Wrist, Ignore
self.zero_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# Indexes: Fixed_base, Ax_0, Ax_1, seg1, Ax_2, seg2, ax_3, seg3, continuous, wrist
self.zero_angles = [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
@@ -48,11 +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}")
# 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}")
# 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,
@@ -64,7 +64,7 @@ class Arm:
fk_position = fk_matrix[:3, 3]
print(f"[TRY] FK Position for Solution: {fk_position}")
# print(f"[TRY] FK Position for Solution: {fk_position}")
error = np.linalg.norm(target_position - fk_position)
if error > self.ik_tolerance:

307
src/arm_pkg/urdf/arm12.urdf Normal file
View File

@@ -0,0 +1,307 @@
<robot name="robot">
<link name="base_footprint"></link>
<joint name="base_joint" type="fixed">
<parent link="base_footprint" />
<child link="base_link" />
<origin xyz="0 0 0.0004048057655643422" rpy="0 0 0" />
</joint>
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.5 0.5 0.01" />
</geometry>
<material name="base_link-material">
<color rgba="0 0.6038273388475408 1 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.5 0.5 0.01" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="0.17" ixy="0" ixz="0" iyy="0.17" iyz="0" izz="0.05" />
</inertial>
</link>
<joint name="Axis_0_Joint" type="revolute">
<parent link="base_link" />
<child link="Axis_0" />
<origin xyz="0 0 0.035" rpy="0 0 0" />
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/> </joint>
<link name="Axis_0">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.15" length="0.059" />
</geometry>
<material name="Axis_0-material">
<color rgba="0.3515325994898463 0.4735314961384573 0.9301108583738498 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.15" length="0.059" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
</inertial>
</link>
<joint name="Axis_1_Joint" type="revolute">
<parent link="Axis_0" />
<child link="Axis_1" />
<origin xyz="0 0 0.11189588647115647" rpy="1.5707963267948963 0 0" />
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="-1.7" upper="1.7" velocity="0.5"/> </joint>
<link name="Axis_1">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.082" length="0.1" />
</geometry>
<material name="Axis_1-material">
<color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.082" length="0.1" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
</inertial>
</link>
<joint name="Axis_1_to_Segment_1" type="fixed">
<parent link="Axis_1" />
<child link="Segment_1" />
<origin xyz="0 0.2350831500270899 0" rpy="-1.5707963267948963 0 0" />
</joint>
<link name="Segment_1">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.025" length="0.473" />
</geometry>
<material name="Segment_1-material">
<color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.025" length="0.473" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
</inertial>
</link>
<joint name="Axis_2_Joint" type="revolute">
<parent link="Segment_1" />
<child link="Axis_2" />
<origin xyz="0 -5.219894517229704e-17 0.2637568842473722" rpy="1.5707963267948963 0 0" />
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/> </joint>
<link name="Axis_2">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.055" length="0.1" />
</geometry>
<material name="Axis_2-material">
<color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.055" length="0.1" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
</inertial>
</link>
<joint name="Axis_2_to_Segment_2" type="fixed">
<parent link="Axis_2" />
<child link="Segment_2" />
<origin xyz="0 0.19535682173790003 0" rpy="-1.5707963267948963 0 0" />
</joint>
<link name="Segment_2">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.025" length="0.393" />
</geometry>
<material name="Segment_2-material">
<color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.025" length="0.393" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
</inertial>
</link>
<joint name="Axis_3_Joint" type="revolute">
<parent link="Segment_2" />
<child link="Axis_3" />
<origin xyz="0 -4.337792830220178e-17 0.199625776257357" rpy="1.5707963267948963 0 0" />
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/> </joint>
<link name="Axis_3">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.05" length="0.1" />
</geometry>
<material name="Axis_3-material">
<color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.05" length="0.1" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
</inertial>
</link>
<joint name="Axis_3_to_Segment_3" type="fixed">
<parent link="Axis_3" />
<child link="Segment_3" />
<origin xyz="0 0.06725724726912972 0" rpy="-1.5707963267948963 0 0" />
</joint>
<link name="Segment_3">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.025" length="0.135" />
</geometry>
<material name="Segment_3-material">
<color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.025" length="0.135" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
</inertial>
</link>
<joint name="Wrist_Joint" type="revolute">
<parent link="Segment_3" />
<child link="Axis_4" />
<origin xyz="0 0 0.0655808825338593" rpy="0 1.5707963267948966 0" />
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/> </joint>
<link name="Axis_4">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.03" length="0.075" />
</geometry>
<material name="Axis_4-material">
<color rgba="0.23455058215026167 0.9301108583738498 0.21952619971859377 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.03" length="0.075" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
</inertial>
</link>
<joint name="Continuous_Joint" type="revolute">
<parent link="Axis_4" />
<child link="Axis_4_C" />
<origin xyz="0.0009533507860803557 0 0" rpy="0 -1.5707963267948966 0" />
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5"/>
</joint>
<link name="Axis_4_C">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.02" length="0.01" />
</geometry>
<material name="Axis_4_C-material">
<color rgba="0.006048833020386069 0.407240211891531 0.15592646369776456 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.02" length="0.01" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
</inertial>
</link>
<joint name="Axis_4_C_to_Effector" type="fixed">
<parent link="Axis_4_C" />
<child link="Effector" />
<origin xyz="0 0 0.06478774571448076" rpy="0 0 0" />
</joint>
<link name="Effector">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.05 0.01 0.135" />
</geometry>
<material name="Effector-material">
<color rgba="0.2746773120495699 0.01680737574872402 0.5711248294565854 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.01 0.01 0.135" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
</inertial>
</link>
</robot>