<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from robot.urdf.pretty.xacro        | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!--
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] ]
-->
<robot name="universal_robot">
  <!-- Inertia parameters -->
  <!-- Invented number, only matters for simulator -->
  <!-- 0.11336 - 0.089159 =  -->
  <!-- 0.119 is not half of 0.39225, is this really correct? -->
  <!-- 0.0018 + 0.10915 = 0.110949 -->
  <!-- 0.01634 + 0.09465 = 0.11099-->
  <!-- Kinematic model -->
  <!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 -->
  <!-- CAD measured -->
  <!-- CAD measured -->
  <!-- In CAD this distance is 0.930, but in the spec it is 0.09465 -->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- Collision model -->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- manually measured -->
  <link name="base_link">
    <visual>
      <geometry>
        <mesh filename="package://ur5_description/meshes/Base.dae"/>
      </geometry>
      <origin rpy="0.0 0.0 2.3561944875" xyz="0.0 0.0 0.003"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur5_description/meshes/Base.dae"/>
      </geometry>
      <origin rpy="0.0 0.0 2.3561944875" xyz="0.0 0.0 0.003"/>
    </collision>
    <inertial>
      <mass value="4.0"/>
      <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 rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/>
    <axis xyz="0.0 0.0 1.0"/>
    <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/>
    <dynamics damping="0.1" friction="0.1"/>
  </joint>
  <link name="shoulder_link">
    <visual>
      <geometry>
        <mesh filename="package://ur5_description/meshes/Shoulder.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur5_description/meshes/Shoulder.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="3.7"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.00193 -0.02561"/>
    </inertial>
  </link>
  <joint name="shoulder_lift_joint" type="revolute">
    <parent link="shoulder_link"/>
    <child link="upper_arm_link"/>
    <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.13585 0.0"/>
    <axis xyz="0.0 1.0 0.0"/>
    <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/>
    <dynamics damping="0.1" friction="0.1"/>
  </joint>
  <link name="upper_arm_link">
    <visual>
      <geometry>
        <mesh filename="package://ur5_description/meshes/UpperArm.dae"/>
      </geometry>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur5_description/meshes/UpperArm.dae"/>
      </geometry>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
    </collision>
    <inertial>
      <mass value="8.393"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.024201 0.2125"/>
    </inertial>
  </link>
  <joint name="elbow_joint" type="revolute">
    <parent link="upper_arm_link"/>
    <child link="forearm_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1197 0.425"/>
    <axis xyz="0.0 1.0 0.0"/>
    <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/>
    <dynamics damping="0.1" friction="0.1"/>
  </joint>
  <link name="forearm_link">
    <visual>
      <geometry>
        <mesh filename="package://ur5_description/meshes/Forearm.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur5_description/meshes/Forearm.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="2.275"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0265 0.11993"/>
      <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 rpy="0.0 1.570796325 0.0" xyz="0.0 0.0 0.39225"/>
    <axis xyz="0.0 1.0 0.0"/>
    <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/>
    <dynamics damping="0.1" friction="0.1"/>
  </joint>
  <link name="wrist_1_link">
    <visual>
      <geometry>
        <mesh filename="package://ur5_description/meshes/Wrist1.dae"/>
      </geometry>
      <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.093 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur5_description/meshes/Wrist1.dae"/>
      </geometry>
      <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.093 0.0"/>
    </collision>
    <inertial>
      <mass value="1.219"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.110949 0.01634"/>
      <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 rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0"/>
    <axis xyz="0.0 0.0 1.0"/>
    <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/>
    <dynamics damping="0.1" friction="0.1"/>
  </joint>
  <link name="wrist_2_link">
    <visual>
      <geometry>
        <mesh filename="package://ur5_description/meshes/Wrist2.dae"/>
      </geometry>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur5_description/meshes/Wrist2.dae"/>
      </geometry>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
    </collision>
    <inertial>
      <mass value="1.219"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0018 0.11099"/>
      <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 rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
    <axis xyz="0.0 1.0 0.0"/>
    <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/>
    <dynamics damping="0.1" friction="0.1"/>
  </joint>
  <link name="wrist_3_link">
    <visual>
      <geometry>
        <mesh filename="package://ur5_description/meshes/Wrist3.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur5_description/meshes/Wrist3.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1879"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.001159 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 rpy="0.0 0.0 1.570796325" xyz="0.0 0.0823 0.0"/>
  </joint>
  <link name="ee_link"/>
  <!-- Extra links for collision model -->
  <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="0.2" radius="0.06"/>
      </geometry>
      <origin rpy="-1.570796325 0.0 0.0" xyz="0.0 -0.035 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="0.2" radius="0.06"/>
      </geometry>
      <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.035 0.425"/>
    </collision>
  </link>
  <gazebo reference="universal_robot">
    <material>Gazebo/Blue</material>
  </gazebo>
</robot>