367 lines
16 KiB
XML
367 lines
16 KiB
XML
<?xml version="1.0"?>
|
|
<robot xmlns:xacro="http://ros.org/wiki/xacro">
|
|
|
|
<!--
|
|
Author: Kelsey Hawkins
|
|
Contributers: Jimmy Da Silva, Ajit Krisshna N L, Muhammad Asif Rana
|
|
-->
|
|
|
|
<xacro:include filename="$(find ur_e_description)/urdf/ur.transmission.xacro" />
|
|
<xacro:include filename="$(find ur_e_description)/urdf/ur.gazebo.xacro" />
|
|
|
|
<xacro:macro name="cylinder_inertial" params="radius length mass *origin">
|
|
<inertial>
|
|
<mass value="${mass}" />
|
|
<xacro:insert_block name="origin" />
|
|
<inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
|
|
iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
|
|
izz="${0.5 * mass * radius * radius}" />
|
|
</inertial>
|
|
</xacro:macro>
|
|
|
|
<xacro:macro name="ur10e_robot" params="prefix joint_limited
|
|
shoulder_pan_lower_limit:=${-pi} shoulder_pan_upper_limit:=${pi}
|
|
shoulder_lift_lower_limit:=${-pi} shoulder_lift_upper_limit:=${pi}
|
|
elbow_joint_lower_limit:=${-pi} elbow_joint_upper_limit:=${pi}
|
|
wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
|
|
wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
|
|
wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}
|
|
transmission_hw_interface:=hardware_interface/PositionJointInterface
|
|
safety_limits:=false safety_pos_margin:=0.15
|
|
safety_k_position:=20">
|
|
|
|
<!-- Inertia parameters -->
|
|
<xacro:property name="base_mass" value="4.0" /> <!-- This mass might be incorrect -->
|
|
<xacro:property name="shoulder_mass" value="7.778" />
|
|
<xacro:property name="upper_arm_mass" value="12.93" />
|
|
<xacro:property name="forearm_mass" value="3.87" />
|
|
<xacro:property name="wrist_1_mass" value="1.96" />
|
|
<xacro:property name="wrist_2_mass" value="1.96" />
|
|
<xacro:property name="wrist_3_mass" value="0.202" />
|
|
|
|
<!-- These parameters are borrowed from the urcontrol.conf file
|
|
but are not verified for the correct permutation.
|
|
The permutation was guessed by looking at the ur5e parameters.
|
|
Serious use of these parameters needs further inspection. -->
|
|
<xacro:property name="shoulder_cog" value="0.00008 0.00244 -0.037" />
|
|
<xacro:property name="upper_arm_cog" value="0.00001 0.15061 0.38757" />
|
|
<xacro:property name="forearm_cog" value="-0.00012 0.06112 0.1984" />
|
|
<xacro:property name="wrist_1_cog" value="-0.00021 -0.00112 0.02269" />
|
|
<xacro:property name="wrist_2_cog" value="-0.00021 0.00112 0.002269" />
|
|
<xacro:property name="wrist_3_cog" value="0 -0.001156 -0.00149" />
|
|
|
|
<!-- Kinematic model -->
|
|
<!-- Properties from urcontrol.conf -->
|
|
<xacro:property name="d1" value="0.181" />
|
|
<xacro:property name="a2" value="-0.613" />
|
|
<xacro:property name="a3" value="-0.571" />
|
|
<xacro:property name="d4" value="0.135" />
|
|
<xacro:property name="d5" value="0.120" />
|
|
<xacro:property name="d6" value="0.117" />
|
|
|
|
<!-- Arbitrary offsets for shoulder/elbow joints -->
|
|
<xacro:property name="shoulder_offset" value="0.176" />
|
|
<xacro:property name="elbow_offset" value="-0.137" />
|
|
|
|
<!-- link lengths used in model -->
|
|
<xacro:property name="shoulder_height" value="${d1}" />
|
|
<xacro:property name="upper_arm_length" value="${-a2}" />
|
|
<xacro:property name="forearm_length" value="${-a3}" />
|
|
<xacro:property name="wrist_1_length" value="${d4}" />
|
|
<xacro:property name="wrist_2_length" value="${d5}" />
|
|
<xacro:property name="wrist_3_length" value="${d6}" />
|
|
|
|
<link name="${prefix}base_link" >
|
|
<visual>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/visual/base.dae"/>
|
|
</geometry>
|
|
<material name="LightGrey">
|
|
<color rgba="0.7 0.7 0.7 1.0"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/collision/base.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<xacro:cylinder_inertial radius="0.075" length="0.038" mass="${base_mass}">
|
|
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
|
|
</xacro:cylinder_inertial>
|
|
</link>
|
|
|
|
<joint name="${prefix}shoulder_pan_joint" type="revolute">
|
|
<parent link="${prefix}base_link" />
|
|
<child link = "${prefix}shoulder_link" />
|
|
<origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" />
|
|
<axis xyz="0 0 1" />
|
|
<xacro:unless value="${joint_limited}">
|
|
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="3.14"/>
|
|
<xacro:if value="${safety_limits}">
|
|
<safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
|
|
</xacro:if>
|
|
</xacro:unless>
|
|
<xacro:if value="${joint_limited}">
|
|
<limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}" effort="330.0" velocity="3.14"/>
|
|
<xacro:if value="${safety_limits}">
|
|
<safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
|
|
</xacro:if>
|
|
</xacro:if>
|
|
<dynamics damping="0.0" friction="0.0"/>
|
|
</joint>
|
|
|
|
<link name="${prefix}shoulder_link">
|
|
<visual>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/visual/shoulder.dae"/>
|
|
</geometry>
|
|
<material name="LightGrey">
|
|
<color rgba="0.7 0.7 0.7 1.0"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/collision/shoulder.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<xacro:cylinder_inertial radius="0.075" length="0.178" mass="${shoulder_mass}">
|
|
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
|
|
</xacro:cylinder_inertial>
|
|
</link>
|
|
|
|
<joint name="${prefix}shoulder_lift_joint" type="revolute">
|
|
<parent link="${prefix}shoulder_link" />
|
|
<child link = "${prefix}upper_arm_link" />
|
|
<origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />
|
|
<axis xyz="0 1 0" />
|
|
<xacro:unless value="${joint_limited}">
|
|
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="3.14"/>
|
|
<xacro:if value="${safety_limits}">
|
|
<safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
|
|
</xacro:if>
|
|
</xacro:unless>
|
|
<xacro:if value="${joint_limited}">
|
|
<limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}" effort="330.0" velocity="3.14"/>
|
|
<xacro:if value="${safety_limits}">
|
|
<safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
|
|
</xacro:if>
|
|
</xacro:if>
|
|
<dynamics damping="0.0" friction="0.0"/>
|
|
</joint>
|
|
|
|
<link name="${prefix}upper_arm_link">
|
|
<visual>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/visual/upperarm.dae"/>
|
|
</geometry>
|
|
<material name="LightGrey">
|
|
<color rgba="0.7 0.7 0.7 1.0"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/collision/upperarm.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<xacro:cylinder_inertial radius="0.075" length="${-a2}" mass="${upper_arm_mass}">
|
|
<origin xyz="0.0 0.0 ${-a2/2.0}" rpy="0 0 0" />
|
|
</xacro:cylinder_inertial>
|
|
</link>
|
|
|
|
<joint name="${prefix}elbow_joint" type="revolute">
|
|
<parent link="${prefix}upper_arm_link" />
|
|
<child link = "${prefix}forearm_link" />
|
|
<origin xyz="0.0 ${elbow_offset} ${upper_arm_length}" rpy="0.0 0.0 0.0" />
|
|
<axis xyz="0 1 0" />
|
|
<xacro:unless value="${joint_limited}">
|
|
<limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.14"/>
|
|
<xacro:if value="${safety_limits}">
|
|
<safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
|
|
</xacro:if>
|
|
</xacro:unless>
|
|
<xacro:if value="${joint_limited}">
|
|
<limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}" effort="150.0" velocity="3.14"/>
|
|
<xacro:if value="${safety_limits}">
|
|
<safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
|
|
</xacro:if>
|
|
</xacro:if>
|
|
<dynamics damping="0.0" friction="0.0"/>
|
|
</joint>
|
|
|
|
<link name="${prefix}forearm_link">
|
|
<visual>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/visual/forearm.dae"/>
|
|
</geometry>
|
|
<material name="LightGrey">
|
|
<color rgba="0.7 0.7 0.7 1.0"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/collision/forearm.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<xacro:cylinder_inertial radius="0.075" length="${-a3}" mass="${forearm_mass}">
|
|
<origin xyz="0.0 0.0 ${-a3/2.0}" rpy="0 0 0" />
|
|
</xacro:cylinder_inertial>
|
|
</link>
|
|
|
|
<joint name="${prefix}wrist_1_joint" type="revolute">
|
|
<parent link="${prefix}forearm_link" />
|
|
<child link = "${prefix}wrist_1_link" />
|
|
<origin xyz="0.0 0.0 ${forearm_length}" rpy="0.0 ${pi / 2.0} 0.0" />
|
|
<axis xyz="0 1 0" />
|
|
<xacro:unless value="${joint_limited}">
|
|
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="6.28"/>
|
|
<xacro:if value="${safety_limits}">
|
|
<safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
|
|
</xacro:if>
|
|
</xacro:unless>
|
|
<xacro:if value="${joint_limited}">
|
|
<limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}" effort="54.0" velocity="6.28"/>
|
|
<xacro:if value="${safety_limits}">
|
|
<safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
|
|
</xacro:if>
|
|
</xacro:if>
|
|
<dynamics damping="0.0" friction="0.0"/>
|
|
</joint>
|
|
|
|
<link name="${prefix}wrist_1_link">
|
|
<visual>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/visual/wrist1.dae"/>
|
|
</geometry>
|
|
<material name="LightGrey">
|
|
<color rgba="0.7 0.7 0.7 1.0"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/collision/wrist1.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_1_mass}">
|
|
<origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0 0 0" />
|
|
</xacro:cylinder_inertial>
|
|
</link>
|
|
|
|
<joint name="${prefix}wrist_2_joint" type="revolute">
|
|
<parent link="${prefix}wrist_1_link" />
|
|
<child link = "${prefix}wrist_2_link" />
|
|
<origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0.0 0.0" />
|
|
<axis xyz="0 0 1" />
|
|
<xacro:unless value="${joint_limited}">
|
|
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="6.28"/>
|
|
<xacro:if value="${safety_limits}">
|
|
<safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
|
|
</xacro:if>
|
|
</xacro:unless>
|
|
<xacro:if value="${joint_limited}">
|
|
<limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}" effort="54.0" velocity="6.28"/>
|
|
<xacro:if value="${safety_limits}">
|
|
<safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
|
|
</xacro:if>
|
|
</xacro:if>
|
|
<dynamics damping="0.0" friction="0.0"/>
|
|
</joint>
|
|
|
|
<link name="${prefix}wrist_2_link">
|
|
<visual>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/visual/wrist2.dae"/>
|
|
</geometry>
|
|
<material name="LightGrey">
|
|
<color rgba="0.7 0.7 0.7 1.0"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/collision/wrist2.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_2_mass}">
|
|
<origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0 0 0" />
|
|
</xacro:cylinder_inertial>
|
|
</link>
|
|
|
|
<joint name="${prefix}wrist_3_joint" type="revolute">
|
|
<parent link="${prefix}wrist_2_link" />
|
|
<child link = "${prefix}wrist_3_link" />
|
|
<origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
|
|
<axis xyz="0 1 0" />
|
|
<xacro:unless value="${joint_limited}">
|
|
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="6.28"/>
|
|
<xacro:if value="${safety_limits}">
|
|
<safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
|
|
</xacro:if>
|
|
</xacro:unless>
|
|
<xacro:if value="${joint_limited}">
|
|
<limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" effort="54.0" velocity="6.28"/>
|
|
<xacro:if value="${safety_limits}">
|
|
<safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
|
|
</xacro:if>
|
|
</xacro:if>
|
|
<dynamics damping="0.0" friction="0.0"/>
|
|
</joint>
|
|
|
|
<link name="${prefix}wrist_3_link">
|
|
<visual>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/visual/wrist3.dae"/>
|
|
</geometry>
|
|
<material name="LightGrey">
|
|
<color rgba="0.7 0.7 0.7 1.0"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<geometry>
|
|
<mesh filename="package://ur_e_description/meshes/ur10e/collision/wrist3.stl"/>
|
|
</geometry>
|
|
</collision>
|
|
<xacro:cylinder_inertial radius="0.045" length="0.05" mass="${wrist_3_mass}">
|
|
<origin xyz="0.0 ${wrist_3_length - 0.05/2} 0.0" rpy="${pi/2} 0 0" />
|
|
</xacro:cylinder_inertial>
|
|
</link>
|
|
|
|
<joint name="${prefix}ee_fixed_joint" type="fixed">
|
|
<parent link="${prefix}wrist_3_link" />
|
|
<child link = "${prefix}ee_link" />
|
|
<origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 ${pi/2.0}" />
|
|
</joint>
|
|
|
|
<link name="${prefix}ee_link">
|
|
<collision>
|
|
<geometry>
|
|
<box size="0.01 0.01 0.01"/>
|
|
</geometry>
|
|
<origin rpy="0 0 0" xyz="-0.01 0 0"/>
|
|
</collision>
|
|
</link>
|
|
|
|
<xacro:ur_arm_transmission prefix="${prefix}" />
|
|
<xacro:ur_arm_gazebo prefix="${prefix}" />
|
|
|
|
<!-- ROS base_link to UR 'Base' Coordinates transform -->
|
|
<link name="${prefix}base"/>
|
|
<joint name="${prefix}base_link-base_fixed_joint" type="fixed">
|
|
<!-- NOTE: this rotation is only needed as long as base_link itself is
|
|
not corrected wrt the real robot (ie: rotated over 180
|
|
degrees)
|
|
-->
|
|
<origin xyz="0 0 0" rpy="0 0 ${-pi}"/>
|
|
<parent link="${prefix}base_link"/>
|
|
<child link="${prefix}base"/>
|
|
</joint>
|
|
|
|
<!-- Frame coincident with all-zeros TCP on UR controller -->
|
|
<link name="${prefix}tool0"/>
|
|
<joint name="${prefix}wrist_3_link-tool0_fixed_joint" type="fixed">
|
|
<origin xyz="0 ${wrist_3_length} 0" rpy="${pi/-2.0} 0 0"/>
|
|
<parent link="${prefix}wrist_3_link"/>
|
|
<child link="${prefix}tool0"/>
|
|
</joint>
|
|
|
|
</xacro:macro>
|
|
</robot>
|