mirror of
https://github.com/SHC-ASTRA/rover-ros2.git
synced 2026-02-11 09:20:40 +00:00
Convert angles to radians for IK
This commit is contained in:
Tristan McGinnis
@@ -63,7 +63,7 @@ class SerialRelay(Node):
|
|||||||
|
|
||||||
self.arm_feedback = SocketFeedback()
|
self.arm_feedback = SocketFeedback()
|
||||||
self.digit_feedback = DigitFeedback()
|
self.digit_feedback = DigitFeedback()
|
||||||
self.arm = astra_arm.Arm('arm11.urdf')
|
self.arm = astra_arm.Arm('arm12.urdf')
|
||||||
self.arm_feedback = SocketFeedback()
|
self.arm_feedback = SocketFeedback()
|
||||||
|
|
||||||
########
|
########
|
||||||
@@ -168,22 +168,13 @@ class SerialRelay(Node):
|
|||||||
angles[0] = 0.0
|
angles[0] = 0.0
|
||||||
#
|
#
|
||||||
#
|
#
|
||||||
#
|
|
||||||
#THIS NEEDS TO BE REMOVED LATER
|
#THIS NEEDS TO BE REMOVED LATER
|
||||||
#PLACEHOLDER FOR WRIST VALUE
|
#PLACEHOLDER FOR WRIST VALUE
|
||||||
#
|
#
|
||||||
##
|
|
||||||
#
|
|
||||||
#
|
|
||||||
#
|
#
|
||||||
angles.append(0.0)
|
angles.append(0.0)
|
||||||
#
|
#
|
||||||
#
|
#
|
||||||
#
|
|
||||||
#
|
|
||||||
#
|
|
||||||
##
|
|
||||||
#
|
|
||||||
# Update the arm's current angles
|
# Update the arm's current angles
|
||||||
self.arm.update_angles(angles)
|
self.arm.update_angles(angles)
|
||||||
self.arm_feedback.axis0_angle = angles[0]
|
self.arm_feedback.axis0_angle = angles[0]
|
||||||
|
|||||||
@@ -15,7 +15,7 @@ degree = pi / 180.0
|
|||||||
|
|
||||||
def convert_angles(angles):
|
def convert_angles(angles):
|
||||||
# Converts angles to the format used for the urdf (contains some dummy joints)
|
# 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:
|
class Arm:
|
||||||
@@ -29,8 +29,8 @@ class Arm:
|
|||||||
|
|
||||||
# Arrays for joint states
|
# Arrays for joint states
|
||||||
# Some links in the URDF are static (non-joints), these will remain zero for IK
|
# 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
|
# 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]
|
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.current_angles = self.zero_angles
|
||||||
self.last_angles = self.zero_angles
|
self.last_angles = self.zero_angles
|
||||||
self.ik_angles = self.zero_angles
|
self.ik_angles = self.zero_angles
|
||||||
@@ -48,11 +48,11 @@ class Arm:
|
|||||||
self.target_position = target_position
|
self.target_position = target_position
|
||||||
# Update the target orientation to the current orientation
|
# Update the target orientation to the current orientation
|
||||||
self.update_orientation()
|
self.update_orientation()
|
||||||
print(f"[IK FOR] Target Position: {self.target_position}")
|
# print(f"[IK FOR] Target Position: {self.target_position}")
|
||||||
try:
|
try:
|
||||||
print(f"[TRY] Current Angles: {self.current_angles}")
|
# print(f"[TRY] Current Angles: {self.current_angles}")
|
||||||
print(f"[TRY] Target Position: {self.target_position}")
|
# print(f"[TRY] Target Position: {self.target_position}")
|
||||||
print(f"[TRY] Target Orientation: {self.target_orientation}")
|
# print(f"[TRY] Target Orientation: {self.target_orientation}")
|
||||||
self.ik_angles = self.chain.inverse_kinematics(
|
self.ik_angles = self.chain.inverse_kinematics(
|
||||||
target_position=self.target_position,
|
target_position=self.target_position,
|
||||||
target_orientation=self.target_orientation,
|
target_orientation=self.target_orientation,
|
||||||
@@ -64,7 +64,7 @@ class Arm:
|
|||||||
|
|
||||||
fk_position = fk_matrix[:3, 3]
|
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)
|
error = np.linalg.norm(target_position - fk_position)
|
||||||
if error > self.ik_tolerance:
|
if error > self.ik_tolerance:
|
||||||
|
|||||||
307
src/arm_pkg/urdf/arm12.urdf
Normal file
307
src/arm_pkg/urdf/arm12.urdf
Normal 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>
|
||||||
Reference in New Issue
Block a user