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

Add urdf folder and arm11.urdf

Browse Source
This commit is contained in:
Tristan McGinnis
2025-03-22 11:17:18 -05:00
committed by David
parent ddb6d672ad
commit 0adab485f2
3 changed files with 300 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -13,6 +13,18 @@ from ros2_interfaces_pkg.msg import ArmIK
from ros2_interfaces_pkg.msg import SocketFeedback
from ros2_interfaces_pkg.msg import DigitFeedback
# IK-Related imports
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
serial_pub = None
thread = None
@@ -43,8 +55,39 @@ class SerialRelay(Node):
self.arm_feedback = SocketFeedback()
self.digit_feedback = DigitFeedback()
########
# Feedback and IK vars
########
# Misc
degree = pi / 180.0
urdf_file_name = 'arm11.urdf'
ik_tolerance = 1e-3 #Tolerance (in meters) to determine if solution is valid
# URDF file path
urdf = os.path.join(get_package_share_directory('ik_pkg'), urdf_file_name)
# IKpy Chain
arm_chain = Chain.from_urdf_file(urdf)
# 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
zero_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
current_angles = zero_angles
last_angles = zero_angles
ik_angles = zero_angles
target_position = [0.0, 0.0, 0.0]
target_orientation = [] # Effector orientation desired at target position.
# Generally orientation for the effector is modified manually by the operator.
########
# Interface with MCU
# Search for ports IF in 'arm' (standalone) and not 'anchor' mode
########
if self.launch_mode == 'arm':
# Loop through all serial devices on the computer to check for the MCU
self.port = None
@@ -123,8 +166,6 @@ class SerialRelay(Node):
self.ser.close()
pass
def send_ik(self, msg):
pass
def send_manual(self, msg: ArmManual):
axis0 = msg.axis0
@@ -278,6 +319,20 @@ class SerialRelay(Node):
self.get_logger().info("Invalid voltage feedback input format")
def send_ik(self, msg):
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
@staticmethod
def list_serial_ports():
return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")

4
src/arm_pkg/setup.py
View File

@@ -1,4 +1,6 @@
from setuptools import find_packages, setup
import os
from glob import glob
package_name = 'arm_pkg'
@@ -10,6 +12,8 @@ setup(
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
(os.path.join('share', package_name), glob('urdf/*')),
],
install_requires=['setuptools'],
zip_safe=True,

239
src/arm_pkg/urdf/arm11.urdf Normal file
View File

@@ -0,0 +1,239 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from omni_description/robots/omnipointer_arm_only.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="omnipointer" xmlns:xacro="http://ros.org/wiki/xacro">
<!--MX64trntbl + MX106: ? + 0.153 -->
<!-- singleaxismount + 2.5girder + singleaxismount + base + MX106: 0.007370876 + 0.0226796 + 0.007370876 + ? + 0.153 -->
<!-- frame106 + singleaxismount + adjustgirder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.110563 + 0.00737088 + ? + 0.077 -->
<!-- frame28 + singleaxismount + 5girder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.0368544 + 0.00737088 + ? + 0.077 -->
<!-- frame28 + singleaxismount + 2.5girder + singleaxismount + tip: 0.0907185 + 0.00737088 + 0.0226796 + 0.00737088 + ? -->
<material name="omni/Blue">
<color rgba="0 0 0.8 1"/>
</material>
<material name="omni/Red">
<color rgba="1 0 0 1"/>
</material>
<material name="omni/Green">
<color rgba="0 1 0 1"/>
</material>
<material name="omni/Yellow">
<color rgba="1 1 0 1"/>
</material>
<material name="omni/LightGrey">
<color rgba="0.6 0.6 0.6 1"/>
</material>
<material name="omni/DarkGrey">
<color rgba="0.4 0.4 0.4 1"/>
</material>
<!-- Now we can start using the macros xacro:included above to define the actual omnipointer -->
<!-- The first use of a macro. This one was defined in youbot_base/base.urdf.xacro above.
A macro like this will expand to a set of link and joint definitions, and to additional
Gazebo-related extensions (sensor plugins, etc). The macro takes an argument, name,
that equals "base", and uses it to generate names for its component links and joints
(e.g., base_link). The xacro:included origin block is also an argument to the macro. By convention,
the origin block defines where the component is w.r.t its parent (in this case the parent
is the world frame). For more, see http://www.ros.org/wiki/xacro -->
<!-- foot for arm-->
<link name="base_link">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="10.0"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
</link>
<!-- joint between base_link and arm_0_link -->
<joint name="arm_joint_0" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base_link"/>
<child link="arm_link_0"/>
</joint>
<link name="arm_link_0">
<visual>
<origin rpy="0 0 0" xyz="0 0 0.02725"/>
<geometry>
<box size="0.1143 0.1143 0.0545"/>
</geometry>
<material name="omni/LightGrey"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.02725"/>
<geometry>
<box size="0.1143 0.1143 0.0545"/>
</geometry>
</collision>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0 0 0.02725"/>
<mass value="0.2"/>
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
<inertia ixx="0.000267245666667" ixy="0" ixz="0" iyy="0.000267245666667" iyz="0" izz="0.000435483"/>
</inertial>
</link>
<joint name="arm_joint_1" type="revolute">
<parent link="arm_link_0"/>
<child link="arm_link_1"/>
<dynamics damping="3.0" friction="0.3"/>
<limit effort="30.0" lower="-3.1415926535" upper="3.1415926535" velocity="5.0"/>
<origin rpy="0 0 0" xyz="0 0 0.0545"/>
<axis xyz="0 0 1"/>
</joint>
<link name="arm_link_1">
<visual>
<origin rpy="0 0 0" xyz="0 0 0.075"/>
<geometry>
<box size="0.0402 0.05 0.15"/>
</geometry>
<material name="omni/Blue"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.075"/>
<geometry>
<box size="0.0402 0.05 0.15"/>
</geometry>
</collision>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0 0 0.075"/>
<mass value="0.190421352"/>
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
<inertia ixx="0.000279744834534" ixy="0" ixz="0" iyy="0.000265717763008" iyz="0" izz="6.53151584738e-05"/>
</inertial>
</link>
<joint name="arm_joint_2" type="revolute">
<parent link="arm_link_1"/>
<child link="arm_link_2"/>
<dynamics damping="3.0" friction="0.3"/>
<limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
<origin rpy="0 0 0" xyz="0 0 0.15"/>
<axis xyz="0 1 0"/>
</joint>
<link name="arm_link_2">
<visual>
<origin rpy="0 0 0" xyz="0 0 0.2355"/>
<geometry>
<box size="0.0356 0.05 0.471"/>
</geometry>
<material name="omni/Red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.2355"/>
<geometry>
<box size="0.0356 0.05 0.471"/>
</geometry>
</collision>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0 0 0.2355"/>
<mass value="0.29302326"/>
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
<inertia ixx="0.00251484771035" ixy="0" ixz="0" iyy="0.00248474836108" iyz="0" izz="9.19936757328e-05"/>
</inertial>
</link>
<joint name="arm_joint_3" type="revolute">
<parent link="arm_link_2"/>
<child link="arm_link_3"/>
<dynamics damping="3.0" friction="0.3"/>
<limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
<origin rpy="0 0 0" xyz="0 0 0.471"/>
<axis xyz="0 1 0"/>
</joint>
<link name="arm_link_3">
<visual>
<origin rpy="0 0 0" xyz="0 0 0.1885"/>
<geometry>
<box size="0.0356 0.05 0.377"/>
</geometry>
<material name="omni/Yellow"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.1885"/>
<geometry>
<box size="0.0356 0.05 0.377"/>
</geometry>
</collision>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0 0 0.1885"/>
<mass value="0.21931466"/>
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
<inertia ixx="0.000791433503053" ixy="0" ixz="0" iyy="0.000768905501178" iyz="0" izz="6.88531064581e-05"/>
</inertial>
</link>
<joint name="arm_joint_4" type="revolute">
<parent link="arm_link_3"/>
<child link="arm_link_4"/>
<dynamics damping="3.0" friction="0.3"/>
<limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
<origin rpy="0 0 0" xyz="0 0 0.377"/>
<axis xyz="0 1 0"/>
</joint>
<link name="arm_link_4">
<visual>
<origin rpy="0 0 0" xyz="0 0 0.066"/>
<geometry>
<box size="0.0356 0.05 0.132"/>
</geometry>
<material name="omni/Green"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.066"/>
<geometry>
<box size="0.0356 0.05 0.132"/>
</geometry>
</collision>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0 0 0.066"/>
<mass value="0.15813986"/>
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
<inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
</inertial>
</link>
<joint name="arm_joint_5" type="revolute">
<parent link="arm_link_4"/>
<child link="arm_link_5"/>
<dynamics damping="3.0" friction="0.3"/>
<limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
<origin rpy="0 0 0" xyz="0 0 0.132"/>
<axis xyz="1 0 0"/>
</joint>
<link name="arm_link_5">
<visual>
<origin rpy="0 0 0" xyz="0 0 0.124"/>
<geometry>
<box size="0.0356 0.05 0.248"/>
</geometry>
<material name="omni/Blue"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.124"/>
<geometry>
<box size="0.0356 0.05 0.248"/>
</geometry>
</collision>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0 0 0.124"/>
<mass value="0.15813986"/>
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
<inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
</inertial>
</link>
<joint name="arm_joint_6" type="fixed">
<parent link="arm_link_5"/>
<child link="arm_link_6"/>
<origin rpy="0 0 0" xyz="0 0 0.248"/>
</joint>
<link name="arm_link_6">
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1e-12"/>
<!-- box inertia: 1/12*m(y^2+z^2), ... -->
<inertia ixx="1e-12" ixy="0" ixz="0" iyy="1e-12" iyz="0" izz="1e-12"/>
</inertial>
</link>
<!-- END OF ARM LINKS/JOINTS -->
</robot>