575 lines
14 KiB
XML
575 lines
14 KiB
XML
<?xml version="1.0" encoding="UTF-8"?>
|
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
|
<xacro:macro name="rbs_arm_vacuum"
|
|
params="namespace parent *origin tf_prefix hardware gripper_name controllers">
|
|
|
|
<xacro:include filename="$(find rbs_mill_assist)/urdf/sensors.xacro"></xacro:include>
|
|
<xacro:include filename="$(find rbs_arm)/urdf/inc/joint_hardware.xacro" />
|
|
|
|
<joint name="${tf_prefix}parent_connection" type="fixed">
|
|
<xacro:insert_block name="origin" />
|
|
<parent link="${parent}" />
|
|
<child link="base_link" />
|
|
</joint>
|
|
|
|
<link
|
|
name="base_link">
|
|
<inertial>
|
|
<origin
|
|
xyz="0.00017379 -0.0030159 0.045999"
|
|
rpy="0 0 0" />
|
|
<mass
|
|
value="0.72132" />
|
|
<inertia
|
|
ixx="0.0013803"
|
|
ixy="-8.9867E-06"
|
|
ixz="-8.5813E-06"
|
|
iyy="0.0018444"
|
|
iyz="-5.4408E-05"
|
|
izz="0.0023998" />
|
|
</inertial>
|
|
<visual>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh filename="file://$(find rbs_mill_assist)/meshes/base_link.STL"></mesh>
|
|
</geometry>
|
|
<material
|
|
name="">
|
|
<color
|
|
rgba="0.75294 0.75294 0.75294 1" />
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/base_link.STL" />
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<link
|
|
name="Link1">
|
|
<inertial>
|
|
<origin
|
|
xyz="0.0045937 0.0021781 0.088722"
|
|
rpy="0 0 0" />
|
|
<mass
|
|
value="0.48715" />
|
|
<inertia
|
|
ixx="0.0016524"
|
|
ixy="9.9556E-07"
|
|
ixz="2.6451E-05"
|
|
iyy="0.0019476"
|
|
iyz="-5.7271E-06"
|
|
izz="0.0008719" />
|
|
</inertial>
|
|
<visual>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link1.STL" />
|
|
</geometry>
|
|
<material
|
|
name="">
|
|
<color
|
|
rgba="0.75294 0.75294 0.75294 1" />
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link1.STL" />
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint
|
|
name="ax1"
|
|
type="revolute">
|
|
<origin
|
|
xyz="0 -0.0002 0.13115"
|
|
rpy="0 0 0" />
|
|
<parent
|
|
link="base_link" />
|
|
<child
|
|
link="Link1" />
|
|
<axis
|
|
xyz="0 0 1" />
|
|
<limit
|
|
lower="-6.2832"
|
|
upper="6.2832"
|
|
effort="30"
|
|
velocity="10" />
|
|
</joint>
|
|
<link
|
|
name="Link2">
|
|
<inertial>
|
|
<origin
|
|
xyz="3.2673E-06 -0.004271 0.045945"
|
|
rpy="0 0 0" />
|
|
<mass
|
|
value="0.38011" />
|
|
<inertia
|
|
ixx="0.0013451"
|
|
ixy="-1.3296E-08"
|
|
ixz="-4.8961E-05"
|
|
iyy="0.0014979"
|
|
iyz="4.3965E-07"
|
|
izz="0.00041688" />
|
|
</inertial>
|
|
<visual>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link2.STL" />
|
|
</geometry>
|
|
<material
|
|
name="">
|
|
<color
|
|
rgba="0.75294 0.75294 0.75294 1" />
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link2.STL" />
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint
|
|
name="ax2"
|
|
type="revolute">
|
|
<origin
|
|
xyz="0 0 0.15465"
|
|
rpy="0 0 0" />
|
|
<parent
|
|
link="Link1" />
|
|
<child
|
|
link="Link2" />
|
|
<axis
|
|
xyz="1 0 0" />
|
|
<limit
|
|
lower="-2.2689"
|
|
upper="1.7453"
|
|
effort="30"
|
|
velocity="10" />
|
|
</joint>
|
|
<link
|
|
name="Link3">
|
|
<inertial>
|
|
<origin
|
|
xyz="-0.0044385 0.00040477 0.0403"
|
|
rpy="0 0 0" />
|
|
<mass
|
|
value="0.39622" />
|
|
<inertia
|
|
ixx="0.00082818"
|
|
ixy="-3.1574E-06"
|
|
ixz="1.2935E-05"
|
|
iyy="0.0010343"
|
|
iyz="1.9002E-06"
|
|
izz="0.0006516" />
|
|
</inertial>
|
|
<visual>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link3.STL" />
|
|
</geometry>
|
|
<material
|
|
name="">
|
|
<color
|
|
rgba="0.75294 0.75294 0.75294 1" />
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link3.STL" />
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint
|
|
name="ax3"
|
|
type="revolute">
|
|
<origin
|
|
xyz="0 0 0.092"
|
|
rpy="0 0 0" />
|
|
<parent
|
|
link="Link2" />
|
|
<child
|
|
link="Link3" />
|
|
<axis
|
|
xyz="1 0 0" />
|
|
<limit
|
|
lower="-2.2689"
|
|
upper="1.7453"
|
|
effort="30"
|
|
velocity="10" />
|
|
</joint>
|
|
<link
|
|
name="Link4">
|
|
<inertial>
|
|
<origin
|
|
xyz="0.0012006 -0.0085317 0.066006"
|
|
rpy="0 0 0" />
|
|
<mass
|
|
value="0.22302" />
|
|
<inertia
|
|
ixx="0.0004366"
|
|
ixy="4.6943E-06"
|
|
ixz="-1.1269E-05"
|
|
iyy="0.00049426"
|
|
iyz="-5.7406E-07"
|
|
izz="0.00018045" />
|
|
</inertial>
|
|
<visual>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link4.STL" />
|
|
</geometry>
|
|
<material
|
|
name="">
|
|
<color
|
|
rgba="0.75294 0.75294 0.75294 1" />
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link4.STL" />
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint
|
|
name="ax4"
|
|
type="revolute">
|
|
<origin
|
|
xyz="0 0 0.098"
|
|
rpy="0 0 0" />
|
|
<parent
|
|
link="Link3" />
|
|
<child
|
|
link="Link4" />
|
|
<axis
|
|
xyz="0 0 1" />
|
|
<limit
|
|
lower="-6.2832"
|
|
upper="6.2832"
|
|
effort="30"
|
|
velocity="10" />
|
|
</joint>
|
|
<link
|
|
name="Link5">
|
|
<inertial>
|
|
<origin
|
|
xyz="-0.00084186 1.3844E-05 0.068718"
|
|
rpy="0 0 0" />
|
|
<mass
|
|
value="0.55235" />
|
|
<inertia
|
|
ixx="0.00048011"
|
|
ixy="5.3951E-07"
|
|
ixz="-4.1834E-06"
|
|
iyy="0.00074778"
|
|
iyz="5.6449E-06"
|
|
izz="0.00070916" />
|
|
</inertial>
|
|
<visual>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link5.STL" />
|
|
</geometry>
|
|
<material
|
|
name="">
|
|
<color
|
|
rgba="0.75294 0.75294 0.75294 1" />
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link5.STL" />
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint
|
|
name="ax5"
|
|
type="revolute">
|
|
<origin
|
|
xyz="0 0 0.0949"
|
|
rpy="0 0 0" />
|
|
<parent
|
|
link="Link4" />
|
|
<child
|
|
link="Link5" />
|
|
<axis
|
|
xyz="1 0 0" />
|
|
<limit
|
|
lower="-1.7453"
|
|
upper="2.0944"
|
|
effort="30"
|
|
velocity="10" />
|
|
</joint>
|
|
<link
|
|
name="Link6">
|
|
<inertial>
|
|
<origin
|
|
xyz="2.3821E-11 -0.054292 0.018171"
|
|
rpy="0 0 0" />
|
|
<mass
|
|
value="0.41222" />
|
|
<inertia
|
|
ixx="0.00057892"
|
|
ixy="-5.0591E-13"
|
|
ixz="1.1462E-13"
|
|
iyy="8.7903E-05"
|
|
iyz="5.3383E-05"
|
|
izz="0.00063814" />
|
|
</inertial>
|
|
<visual>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link6.STL" />
|
|
</geometry>
|
|
<material
|
|
name="">
|
|
<color
|
|
rgba="0.75294 0.75294 0.75294 1" />
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link6.STL" />
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint
|
|
name="ax6"
|
|
type="revolute">
|
|
<origin
|
|
xyz="0.0012 0 0.10155"
|
|
rpy="0 0 0" />
|
|
<parent
|
|
link="Link5" />
|
|
<child
|
|
link="Link6" />
|
|
<axis
|
|
xyz="0 0 1" />
|
|
<limit
|
|
lower="-6.2832"
|
|
upper="6.2832"
|
|
effort="30"
|
|
velocity="10" />
|
|
</joint>
|
|
<link
|
|
name="Link7">
|
|
<inertial>
|
|
<origin
|
|
xyz="3.46944695195361E-18 -5.55111512312578E-17 -0.00360613836994461"
|
|
rpy="0 0 0" />
|
|
<mass
|
|
value="0.0198935071603267" />
|
|
<inertia
|
|
ixx="1.01905343575873E-06"
|
|
ixy="3.76158192263132E-37"
|
|
ixz="-3.1449831265618E-23"
|
|
iyy="1.01905343575873E-06"
|
|
iyz="6.29843212132371E-23"
|
|
izz="1.43041044988815E-06" />
|
|
</inertial>
|
|
<visual>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link7.STL" />
|
|
</geometry>
|
|
<material
|
|
name="">
|
|
<color
|
|
rgba="1 1 1 0.4" />
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link7.STL" />
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint
|
|
name="Right_point"
|
|
type="fixed">
|
|
<origin
|
|
xyz="0.015 -0.095 0.05"
|
|
rpy="0 0 0" />
|
|
<parent
|
|
link="Link6" />
|
|
<child
|
|
link="Link7" />
|
|
<axis
|
|
xyz="0 0 0" />
|
|
</joint>
|
|
<link
|
|
name="Link8">
|
|
<inertial>
|
|
<origin
|
|
xyz="0 -2.77555756156289E-17 -0.00360613836994461"
|
|
rpy="0 0 0" />
|
|
<mass
|
|
value="0.0198935071603267" />
|
|
<inertia
|
|
ixx="1.01905343575873E-06"
|
|
ixy="0"
|
|
ixz="-2.89418945570099E-23"
|
|
iyy="1.01905343575872E-06"
|
|
iyz="-1.78775100697742E-23"
|
|
izz="1.43041044988815E-06" />
|
|
</inertial>
|
|
<visual>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link8.STL" />
|
|
</geometry>
|
|
<material
|
|
name="">
|
|
<color
|
|
rgba="1 1 1 0.4" />
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin
|
|
xyz="0 0 0"
|
|
rpy="0 0 0" />
|
|
<geometry>
|
|
<mesh
|
|
filename="file://$(find rbs_mill_assist)/meshes/Link8.STL" />
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint
|
|
name="Left_point"
|
|
type="fixed">
|
|
<origin
|
|
xyz="-0.015 -0.095 0.05"
|
|
rpy="0 0 0" />
|
|
<parent
|
|
link="Link6" />
|
|
<child
|
|
link="Link8" />
|
|
<axis
|
|
xyz="0 0 0" />
|
|
</joint>
|
|
<link name="grasp_point"/>
|
|
<joint
|
|
name="grasp_point_joint"
|
|
type="fixed">
|
|
<origin
|
|
xyz="0.0 -0.095 0.05"
|
|
rpy="0 0 0" />
|
|
<parent
|
|
link="Link6" />
|
|
<child
|
|
link="grasp_point" />
|
|
<axis
|
|
xyz="0 0 0" />
|
|
</joint>
|
|
|
|
<xacro:macro name="gazebo_reference" params="joint_name">
|
|
<gazebo reference='${joint_name}'>
|
|
<preserveFixedJoint>true</preserveFixedJoint>
|
|
</gazebo>
|
|
</xacro:macro>
|
|
|
|
<xacro:gazebo_reference joint_name="Right_point"/>
|
|
<xacro:gazebo_reference joint_name="Left_point"/>
|
|
|
|
|
|
<xacro:joint_hardware joint_name="ax1" hardware="${hardware}" initial_joint_position="0.0" />
|
|
<xacro:joint_hardware joint_name="ax2" hardware="${hardware}" initial_joint_position="0.85" />
|
|
<xacro:joint_hardware joint_name="ax3" hardware="${hardware}" initial_joint_position="1.0" />
|
|
<xacro:joint_hardware joint_name="ax4" hardware="${hardware}" initial_joint_position="0.0" />
|
|
<xacro:joint_hardware joint_name="ax5" hardware="${hardware}" initial_joint_position="1.0" />
|
|
<xacro:joint_hardware joint_name="ax6" hardware="${hardware}" initial_joint_position="3.14159" />
|
|
|
|
|
|
<xacro:if value="${hardware=='gazebo'}">
|
|
<!-- <xacro:fts link="tool0" name="fts_sensor" tf_prefix="${tf_prefix}"></xacro:fts> -->
|
|
|
|
<xacro:rgbd parent="Link1" tf_prefix="${tf_prefix}">
|
|
<origin rpy="0.0 0.0 -1.57" xyz="0.0 -0.035 0.03"></origin>
|
|
</xacro:rgbd>
|
|
<gazebo>
|
|
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
|
|
<parameters>$(arg simulation_controllers)</parameters>
|
|
<ros>
|
|
<namespace>${namespace}</namespace>
|
|
</ros>
|
|
</plugin>
|
|
<plugin filename="VacuumGripper" name="rbs_mill_assist::VacuumGripper">
|
|
<link_name>Link8</link_name>
|
|
</plugin>
|
|
</gazebo>
|
|
</xacro:if>
|
|
<gazebo reference="Link8">
|
|
<sensor name='left_sensor_contact' type='contact'>
|
|
<contact>
|
|
<collision>Link8_collision</collision>
|
|
</contact>
|
|
</sensor>
|
|
</gazebo>
|
|
<gazebo reference="Link7">
|
|
<sensor name='right_sensor_contact' type='contact'>
|
|
<contact>
|
|
<collision>Link7_collision</collision>
|
|
</contact>
|
|
</sensor>
|
|
</gazebo>
|
|
</xacro:macro>
|
|
</robot>
|
|
|