Newer
Older
<!--
DH for UR5:
a = [0.00000, -0.42500, -0.39225, 0.00000, 0.00000, 0.0000]
d = [0.089159, 0.00000, 0.00000, 0.10915, 0.09465, 0.0823]
alpha = [ 1.570796327, 0, 0, 1.570796327, -1.570796327, 0 ]
q_home_offset = [0, -1.570796327, 0, -1.570796327, 0, 0]
joint_direction = [-1, -1, 1, 1, 1, 1]
mass = [3.7000, 8.3930, 2.2750, 1.2190, 1.2190, 0.1879]
center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0, 0.0265], [0, -0.0018, 0.01634], [0, 0.0018,0.01634], [0, 0, -0.001159] ]
-->
<property name="pi" value="3.14159265" />
<property name="base_mass" value="10.0" /> <!-- not form spec -->
<property name="shoulder_mass" value="3.7000" />
<property name="upper_arm_mass" value="8.3930" />
<property name="forearm_mass" value="2.2750" />
<property name="wrist_1_mass" value="1.2190" />
<property name="wrist_2_mass" value="1.2190" />
<property name="wrist_3_mass" value="0.1879" />
<property name="shoulder_cog" value="0.0 0.00193 -0.02561" />
<property name="upper_arm_cog" value="0.0 -0.024201 0.2125" /> <!-- 0.11336 - 0.089159 = -->
<property name="forearm_cog" value="0.0 0.0265 0.11993" /> <!-- 0.119 is not half of 0.39225 ?? -->
<property name="wrist_1_cog" value="0.0 0.110949 0.01634" /> <!-- 0.0018 + 0.10915 = 0.110949 -->
<property name="wrist_2_cog" value="0.0 0.0018 0.11099" /> <!-- 0.01634 + 0.09465 = 0.11099-->
<property name="wrist_3_cog" value="0.0 0.001159 0.0" />
Wim Meeussen
committed
<property name="shoulder_height" value="0.089159" />
<property name="shoulder_offset" value="0.13915" /> <!-- sh_off - el_off + wr_1_off = 0.10915 -->
<property name="upper_arm_length" value="0.42500" />
Wim Meeussen
committed
<property name="elbow_offset" value="0.130" /> <!-- manually measured -->
<property name="forearm_length" value="0.39225" />
Wim Meeussen
committed
<property name="wrist_1_length" value="0.100" /> <!-- manually measured -->
<property name="wrist_2_length" value="0.09465" />
<property name="wrist_3_length" value="0.0823" />
Wim Meeussen
committed
<property name="shoulder_radius" value="0.060" /> <!-- manually measured -->
<property name="upper_arm_radius" value="0.054" /> <!-- manually measured -->
<property name="elbow_radius" value="0.060" /> <!-- manually measured -->
<property name="forearm_radius" value="0.038" /> <!-- manually measured -->
<property name="wrist_radius" value="0.035" /> <!-- manually measured -->
<property name="base_collision_length" value="0.160" /> <!-- manually measured -->
<property name="shoulder_collision_length" value="0.200" /> <!-- manually measured -->
<property name="shoulder_collision_offset" value="0.035" /> <!-- manually measured -->
<property name="elbow_collision_length" value="0.200" /> <!-- manually measured -->
<property name="elbow_collision_offset" value="0.035" /> <!-- manually measured -->
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="shoulder_pan_joint" type="revolute">
<parent link="base_link" />
<child link = "shoulder_link" />
<origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" />
<axis xyz="0.0 0.0 1.0" />
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
<dynamics damping="0.1" friction="0.1"/>
</joint>
<link name="shoulder_link">
<visual>
<geometry>
Wim Meeussen
committed
<cylinder length="${base_collision_length}" radius="${shoulder_radius}"/>
Wim Meeussen
committed
<origin xyz="0.0 0.0 ${base_collision_length / 2.0 - shoulder_height}" rpy="0.0 0.0 0.0" />
<collision>
<geometry>
<cylinder length="${shoulder_height}" radius="${shoulder_radius}"/>
</geometry>
<origin xyz="0.0 0.0 ${-shoulder_height/2.0}" rpy="0.0 0.0 0.0" />
</collision>
<inertial>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
<origin xyz="${shoulder_cog}" rpy="0.0 0.0 0.0" />
</link>
<joint name="shoulder_lift_joint" type="revolute">
<parent link="shoulder_link" />
<child link = "upper_arm_link" />
<origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
<dynamics damping="0.1" friction="0.1"/>
</joint>
<link name="upper_arm_link">
<visual>
<geometry>
<mesh filename="package://ur5_description/meshes/UpperArm.dae" />
Wim Meeussen
committed
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<collision>
<geometry>
<cylinder length="${upper_arm_length}" radius="${upper_arm_radius}"/>
</geometry>
<origin xyz="0.0 0.0 ${upper_arm_length/2.0}" rpy="0.0 0.0 0.0" />
</collision>
<inertial>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
<origin xyz="${upper_arm_cog}" rpy="0.0 0.0 0.0" />
</link>
<joint name="elbow_joint" type="revolute">
<parent link="upper_arm_link" />
<child link = "forearm_link" />
Wim Meeussen
committed
<origin xyz="0.0 ${-elbow_offset} ${upper_arm_length}" rpy="0.0 0.0 0.0" />
<axis xyz="0.0 1.0 0.0" />
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
<dynamics damping="0.1" friction="0.1"/>
</joint>
<link name="forearm_link">
<visual>
<geometry>
<cylinder length="${forearm_length}" radius="${forearm_radius}"/>
</geometry>
<origin xyz="0.0 0.0 ${forearm_length/2.0}" rpy="0.0 0.0 0.0" />
</visual>
<collision>
<geometry>
<cylinder length="${forearm_length}" radius="${forearm_radius}"/>
</geometry>
<origin xyz="0.0 0.0 ${forearm_length/2.0}" rpy="0.0 0.0 0.0" />
</collision>
<inertial>
<mass value="${forearm_mass}" />
<origin xyz="${forearm_cog}" rpy="0.0 0.0 0.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="wrist_1_joint" type="revolute">
<parent link="forearm_link" />
<child link = "wrist_1_link" />
<origin xyz="0.0 0.0 ${forearm_length}" rpy="0.0 ${pi / 2.0} 0.0" />
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
<dynamics damping="0.1" friction="0.1"/>
</joint>
<link name="wrist_1_link">
<visual>
<geometry>
<cylinder length="${wrist_1_length}" radius="${wrist_radius}"/>
<origin xyz="0.0 ${wrist_1_length/2.0} 0.0" rpy="${-pi/2.0} 0.0 0.0" />
<collision>
<geometry>
<cylinder length="${wrist_1_length}" radius="${wrist_radius}"/>
</geometry>
<origin xyz="0.0 ${wrist_1_length/2.0} 0.0" rpy="${-pi/2.0} 0.0 0.0" />
<mass value="${wrist_1_mass}" />
<origin xyz="${wrist_1_cog}" rpy="0.0 0.0 0.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="wrist_2_joint" type="revolute">
<parent link="wrist_1_link" />
<child link = "wrist_2_link" />
<origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0.0 0.0" />
<axis xyz="0.0 0.0 1.0" />
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
<dynamics damping="0.1" friction="0.1"/>
</joint>
<link name="wrist_2_link">
<visual>
<geometry>
<cylinder length="${wrist_2_length}" radius="${wrist_radius}"/>
</geometry>
<origin xyz="0.0 0.0 ${wrist_2_length/2.0}" rpy="0.0 0.0 0.0" />
</visual>
<collision>
<geometry>
<cylinder length="${wrist_2_length}" radius="${wrist_radius}"/>
</geometry>
<origin xyz="0.0 0.0 ${wrist_2_length/2.0}" rpy="0.0 0.0 0.0" />
</collision>
<inertial>
<mass value="${wrist_2_mass}" />
<origin xyz="${wrist_2_cog}" rpy="0.0 0.0 0.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="wrist_3_joint" type="revolute">
<parent link="wrist_2_link" />
<child link = "wrist_3_link" />
<origin xyz="0.0 0.0 ${wrist_3_length}" rpy="0.0 0.0 0.0" />
<axis xyz="0.0 1.0 0.0" />
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
<dynamics damping="0.1" friction="0.1"/>
</joint>
<link name="wrist_3_link">
<visual>
<geometry>
<cylinder length="${wrist_3_length}" radius="${wrist_radius}"/>
</geometry>
<origin xyz="0.0 ${wrist_3_length/2.0} 0.0" rpy="${-pi/2.0} 0.0 0.0" />
</visual>
<collision>
<geometry>
<cylinder length="${wrist_3_length}" radius="${wrist_radius}"/>
</geometry>
<origin xyz="0.0 ${wrist_3_length/2.0} 0.0" rpy="${-pi/2.0} 0.0 0.0" />
</collision>
<inertial>
<mass value="${wrist_3_mass}" />
<origin xyz="${wrist_3_cog}" rpy="0.0 0.0 0.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="ee_fixed_joint" type="fixed">
<parent link="wrist_3_link" />
<child link = "ee_link" />
<origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 ${pi/2.0}" />
</joint>
<link name="ee_link">
<visual>
<geometry>
<cylinder length="0.01" radius="${wrist_radius * 1.25}"/>
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 ${-pi/2.0} 0.0" />
</visual>
<cylinder length="0.01" radius="${wrist_radius * 1.25}"/>
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 ${-pi/2.0} 0.0" />
</collision>
Wim Meeussen
committed
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
<joint name="shoulder_collision_joint" type="fixed">
<parent link="upper_arm_link" />
<child link = "shoulder_collision_link" />
</joint>
<link name="shoulder_collision_link">
<collision>
<geometry>
<cylinder length="${shoulder_collision_length}" radius="${shoulder_radius}"/>
</geometry>
<origin xyz="0.0 ${-shoulder_collision_offset} 0.0" rpy="${-pi / 2.0} 0.0 0.0" />
</collision>
</link>
<joint name="elbow_collision_joint" type="fixed">
<parent link="upper_arm_link" />
<child link = "elbow_collision_link" />
</joint>
<link name="elbow_collision_link">
<collision>
<geometry>
<cylinder length="${elbow_collision_length}" radius="${elbow_radius}"/>
</geometry>
<origin xyz="0.0 ${-elbow_collision_offset} ${upper_arm_length}" rpy="${pi / 2.0} 0.0 0.0" />
</collision>
</link>
<gazebo reference="universal_robot">
<material>Gazebo/Blue</material>
</gazebo>