<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from ur5_robot.urdf.xacro           | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="ur5" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!-- common stuff -->
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="ros_control">
      <!--robotNamespace>/</robotNamespace-->
      <!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
    </plugin>
    <!--
    <plugin name="gazebo_ros_power_monitor_controller" filename="libgazebo_ros_power_monitor.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>1.0</updateRate>
      <timeout>5</timeout>
      <powerStateTopic>power_state</powerStateTopic>
      <powerStateRate>10.0</powerStateRate>
      <fullChargeCapacity>87.78</fullChargeCapacity>
      <dischargeRate>-474</dischargeRate>
      <chargeRate>525</chargeRate>
      <dischargeVoltage>15.52</dischargeVoltage>
      <chargeVoltage>16.41</chargeVoltage>
    </plugin>
-->
  </gazebo>
  <!-- ur5 -->
  <!-- Inertia parameters -->
  <!-- This mass might be incorrect -->
  <!-- Kinematic model -->
  <!-- Properties from urcontrol.conf -->
  <!--
    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] ]
  -->
  <!-- Arbitrary offsets for shoulder/elbow joints -->
  <!-- measured from model -->
  <!-- measured from model -->
  <!-- link lengths used in model -->
  <!--property name="shoulder_height" value="0.089159" /-->
  <!--property name="shoulder_offset" value="0.13585" /-->
  <!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 -->
  <!--property name="upper_arm_length" value="0.42500" /-->
  <!--property name="elbow_offset" value="0.1197" /-->
  <!-- CAD measured -->
  <!--property name="forearm_length" value="0.39225" /-->
  <!--property name="wrist_1_length" value="0.093" /-->
  <!-- CAD measured -->
  <!--property name="wrist_2_length" value="0.09465" /-->
  <!-- In CAD this distance is 0.930, but in the spec it is 0.09465 -->
  <!--property name="wrist_3_length" value="0.0823" /-->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- arm -->
     <link name="/base_link"/>
   <joint name="base_link_fixed" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
    <parent link="/base_link"/>
    <child link="base_link"/>
  </joint>

  <link name="base_link">
    <visual>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/visual/base.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/collision/base.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="4.0"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
    </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 1"/>
    <limit effort="150.0" lower="-6.2831853" upper="6.2831853" velocity="3.15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="shoulder_link">
    <visual>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/visual/shoulder.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/collision/shoulder.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="3.7"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" iyy="0.010267495893" iyz="0.0" izz="0.00666"/>
    </inertial>
  </link>
  <joint name="shoulder_lift_joint" type="revolute">
    <parent link="shoulder_link"/>
    <child link="upper_arm_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.13585 0.0"/>
    <axis xyz="0 1 0"/>
    <limit effort="150.0" lower="-6.2831853" upper="6.2831853" velocity="3.15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="upper_arm_link">
    <visual>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/visual/upperarm.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/collision/upperarm.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="8.393"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.28"/>
      <inertia ixx="0.22689067591" ixy="0.0" ixz="0.0" iyy="0.22689067591" iyz="0.0" izz="0.0151074"/>
    </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 1 0"/>
    <limit effort="150.0" lower="-6.2831853" upper="6.2831853" velocity="3.15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="forearm_link">
    <visual>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/visual/forearm.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/collision/forearm.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="2.275"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.25"/>
      <inertia ixx="0.049443313556" ixy="0.0" ixz="0.0" iyy="0.049443313556" iyz="0.0" izz="0.004095"/>
    </inertial>
  </link>
  <joint name="wrist_1_joint" type="revolute">
    <parent link="forearm_link"/>
    <child link="wrist_1_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.39225"/>
    <axis xyz="0 1 0"/>
    <limit effort="28.0" lower="-6.2831853" upper="6.2831853" velocity="3.2"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="wrist_1_link">
    <visual>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/visual/wrist1.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/collision/wrist1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.219"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
    </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 1"/>
    <limit effort="28.0" lower="-6.2831853" upper="6.2831853" velocity="3.2"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="wrist_2_link">
    <visual>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/visual/wrist2.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/collision/wrist2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.219"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
    </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 1 0"/>
    <limit effort="28.0" lower="-6.2831853" upper="6.2831853" velocity="3.2"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="wrist_3_link">
    <visual>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/visual/wrist3.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/collision/wrist3.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1879"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.0171364731454" ixy="0.0" ixz="0.0" iyy="0.0171364731454" iyz="0.0" izz="0.033822"/>
    </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">
    <collision>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
      <origin rpy="0 0 0" xyz="-0.01 0 0"/>
    </collision>
  </link>
  <transmission name="shoulder_pan_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="shoulder_pan_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="shoulder_pan_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="shoulder_lift_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="shoulder_lift_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="shoulder_lift_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="elbow_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="elbow_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="elbow_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="wrist_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wrist_1_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="wrist_1_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="wrist_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wrist_2_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="wrist_2_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="wrist_3_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wrist_3_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="wrist_3_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- nothing to do here at the moment -->
  <!-- ROS base_link to UR 'Base' Coordinates transform -->
  <link name="base"/>
  <joint name="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 rpy="0 0 -3.14159265" xyz="0 0 0"/>
    <parent link="base_link"/>
    <child link="base"/>
  </joint>
  <!-- Frame coincident with all-zeros TCP on UR controller -->
  <link name="tool0"/>
  <joint name="wrist_3_link-tool0_fixed_joint" type="fixed">
    <origin rpy="-1.570796325 0 0" xyz="0 0.0823 0"/>
    <parent link="wrist_3_link"/>
    <child link="tool0"/>
  </joint>
  <!-- <link name="world"/>
  <joint name="world_joint" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
  </joint> -->



<!-- Base of whole end effector - (Connector Plate + Electric Gripper + Fingers + Tips) -->
  <link name="right_gripper_base">
    <inertial>
      <!-- Sum Gripper Mass - for whole end effector, measured from end of plastic cuff -->
      <mass value="0.47"/>
      <origin rpy="0 0 0" xyz="0.0028 -0.0004 0.0343"/>
      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
    </inertial>
  </link>
  <!-- Electric Gripper Base Link -->
  <link name="right_electric_gripper_base">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/electric_gripper/electric_gripper_base.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="-1.57079632679 3.14159265359 0" xyz="0.004 0.0 0.025"/>
      <geometry>
        <cylinder length="0.1" radius="0.029"/>
      </geometry>
      <material name="darkred">
        <color rgba=".5 .1 .1 1"/>
      </material>
    </collision>
    <inertial>
      <origin rpy="-1.57079632679 3.14159265359 0" xyz="0.0 0.0 0.0"/>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>
  <!-- Connector Plate Joint -->
  <joint name="right_connector_plate_base_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.0018"/>
    <parent link="right_gripper_base"/>
    <child link="right_connector_plate_base"/>
  </joint>
  <link name="right_connector_plate_base">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.0"/>
      <inertia ixx="6.9922E-05" ixy="6.1026E-08" ixz="-1.7042E-06" iyy="6.8188E-05" iyz="-1.6118E-08" izz="0.00012392"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/connector_plate/connector_plate.dae"/>
      </geometry>
      <material name="black">
        <color rgba="0.05 0.05 0.05 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0135"/>
      <geometry>
        <cylinder length="0.0302" radius="0.04"/>
      </geometry>
      <material name="darkred">
        <color rgba=".5 .1 .1 1"/>
      </material>
    </collision>
  </link>
  <link name="right_connector_plate_mount"/>
  <joint name="right_connector_plate_mount_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.0176"/>
    <parent link="right_connector_plate_base"/>
    <child link="right_connector_plate_mount"/>
  </joint>
  <!-- Electric Gripper Base joint -->
  <joint name="right_electric_gripper_base_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="right_connector_plate_mount"/>
    <child link="right_electric_gripper_base"/>
  </joint>
  <link name="right_gripper_l_finger">
    <visual>
      <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/electric_gripper/fingers/standard_narrow.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 -3.14159265359" xyz="0 0.01725 0.04"/>
      <geometry>
        <box size="0.01 0.0135 0.075"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 -3.14159265359" xyz="-0.005 -0.003 0.0083"/>
      <geometry>
        <box size="0.01 0.05 0.017"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
  </link>
  <link name="right_gripper_l_finger_tip">
    <visual>
      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/electric_gripper/fingers/basic_soft_tip.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 3.14159265359" xyz="0 -0.0045 -0.015"/>
      <geometry>
        <box size="0.016 0.007 0.037"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
  </link>
  <joint name="right_gripper_l_finger_tip_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.01725 0.075"/>
    <parent link="right_gripper_l_finger"/>
    <child link="right_gripper_l_finger_tip"/>
  </joint>
  <link name="right_gripper_r_finger">
    <visual>
      <origin rpy="0 0 -0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/electric_gripper/fingers/standard_narrow.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 -0.0" xyz="0 -0.01725 0.04"/>
      <geometry>
        <box size="0.01 0.0135 0.075"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 -0.0" xyz="0.005 0.003 0.0083"/>
      <geometry>
        <box size="0.01 0.05 0.017"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 -0.0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
  </link>
  <link name="right_gripper_r_finger_tip">
    <visual>
      <origin rpy="0 0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://sawyer_with_gripper_description/meshes/electric_gripper/fingers/basic_soft_tip.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0.0" xyz="0 0.0045 -0.015"/>
      <geometry>
        <box size="0.016 0.007 0.037"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0.0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
  </link>
  <joint name="right_gripper_r_finger_tip_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 -0.01725 0.075"/>
    <parent link="right_gripper_r_finger"/>
    <child link="right_gripper_r_finger_tip"/>
  </joint>
  <joint name="right_gripper_base_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 -0.005"/>
    <parent link="tool0"/>
    <child link="right_gripper_base"/>
  </joint>
  <joint name="right_gripper_tip_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.14062"/>
    <parent link="right_gripper_base"/>
    <child link="right_gripper_tip"/>
  </joint>
  <!-- Electric Gripper Tip link -->
  <link name="right_gripper_tip">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>

  <joint name="right_gripper_l_finger_joint" type="prismatic">
    <origin rpy="0 0 0" xyz="0.003 -0.0015 0.04552"/>
    <axis xyz="0 1 0"/>
    <parent link="right_electric_gripper_base"/>
    <child link="right_gripper_l_finger"/>
    <limit effort="20.0" lower="0.0" upper="0.020833" velocity="5.0"/>
    <dynamics damping="100.0" friction="500.0"/>
  </joint>
  <joint name="right_gripper_r_finger_joint" type="prismatic">
    <origin rpy="0 0 0" xyz="0.003 0.0015 0.04552"/>
    <axis xyz="0 1 0"/>
    <parent link="right_electric_gripper_base"/>
    <child link="right_gripper_r_finger"/>
    <limit effort="20.0" lower="-0.020833" upper="0.0" velocity="5.0"/>
    <mimic joint="right_gripper_l_finger_joint" multiplier="-1.0"/>
    <dynamics damping="100.0" friction="500.0"/>
  </joint>
</robot>