<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from sisbot.xacro                   | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="ur5_robotiq_140" xmlns:xacro="http://ros.org/wiki/xacro">
  <!--################################################
  ########              arm                    #######
  ####################################################-->

  <link name="base_link">
    <visual>
      <geometry>
        <mesh filename="../meshes/ur5/visual/base.obj"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="../meshes/ur5/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="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="shoulder_link">
    <visual>
      <geometry>
        <mesh filename="../meshes/ur5/visual/shoulder.obj"/>
      </geometry>
      <material name="DarkGray">
        <color rgba="0.5 0.5 0.5 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="../meshes/ur5/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 1.57079632679 0.0" xyz="0.0 0.13585 0.0"/>
    <axis xyz="0 1 0"/>
    <limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="upper_arm_link">
    <visual>
      <geometry>
        <mesh filename="../meshes/ur5/visual/upperarm.obj"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="../meshes/ur5/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="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="forearm_link">
    <visual>
      <geometry>
        <mesh filename="../meshes/ur5/visual/forearm.obj"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="../meshes/ur5/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 1.57079632679 0.0" xyz="0.0 0.0 0.39225"/>
    <axis xyz="0 1 0"/>
    <limit effort="28.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.2"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="wrist_1_link">
    <visual>
      <geometry>
        <mesh filename="../meshes/ur5/visual/wrist1.obj"/>
      </geometry>
      <material name="DarkGray">
        <color rgba="0.5 0.5 0.5 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="../meshes/ur5/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="-3.14159265359" upper="3.14159265359" velocity="3.2"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="wrist_2_link">
    <visual>
      <geometry>
        <mesh filename="../meshes/ur5/visual/wrist2.obj"/>
      </geometry>
      <material name="DarkGray">
        <color rgba="0.5 0.5 0.5 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="../meshes/ur5/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="-3.14159265359" upper="3.14159265359" velocity="3.2"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="wrist_3_link">
    <visual>
      <geometry>
        <mesh filename="../meshes/ur5/visual/wrist3.obj"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="../meshes/ur5/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.57079632679" 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>

  <!--################################################
  ########              gripper                  #####
  ####################################################-->

  <link name="robotiq_arg2f_base_link">
    <inertial>
      <origin rpy="0 0 0" xyz="8.625E-08 -4.6583E-06 0.03145"/>
      <mass value="0.22652"/>
      <inertia ixx="0.00020005" ixy="-4.2442E-10" ixz="-2.9069E-10" iyy="0.00017832" iyz="-3.4402E-08" izz="0.00013478"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_base_link.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.1 0.1 0.1 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_base_link.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="left_outer_knuckle">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331"/>
      <mass value="0.00853198276973456"/>
      <inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_outer_knuckle.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 0.1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_outer_knuckle.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="left_outer_finger">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385"/>
      <mass value="0.022614240507152"/>
      <inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_outer_finger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.1 0.1 0.1 0.2"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_outer_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="left_inner_finger">
    <inertial>
      <origin rpy="0 0 0" xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257"/>
      <mass value="0.0104003125914103"/>
      <inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_inner_finger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.1 0.1 0.1 0.3"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_inner_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="left_inner_finger_pad">
    <inertial>
      <mass value="1."/>
      <inertia ixx="1E-10" ixy="1E-10" ixz="1E-10" iyy="1E-10" iyz="1E-10" izz="1E-10"/>
    </inertial>

    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.027 0.065 0.0075"/>
      </geometry>
      <material name="">
        <color rgba="0 0 1 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.03 0.07 0.0075"/>
      </geometry>
      <material name="">
        <color rgba="0.9 0.0 0.0 1"/>
      </material>
    </collision>
  </link>
  <link name="left_inner_knuckle">
    <inertial>
      <origin rpy="0 0 0" xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166"/>
      <mass value="0.0271177346495152"/>
      <inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_inner_knuckle.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.1 0.1 0.1 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_inner_knuckle.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="right_outer_knuckle">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331"/>
      <mass value="0.00853198276973456"/>
      <inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_outer_knuckle.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_outer_knuckle.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="right_outer_finger">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385"/>
      <mass value="0.022614240507152"/>
      <inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_outer_finger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.1 0.1 0.1 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_outer_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="right_inner_finger">
    <inertial>
      <origin rpy="0 0 0" xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257"/>
      <mass value="0.0104003125914103"/>
      <inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_inner_finger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.1 0.1 0.1 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_inner_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="right_inner_finger_pad">
    <inertial>
      <mass value="1."/>
      <inertia ixx="1E-10" ixy="1E-10" ixz="1E-10" iyy="1E-10" iyz="1E-10" izz="1E-10"/>
    </inertial>

    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.027 0.065 0.0075"/>
      </geometry>
      <material name="">
        <color rgba="1 0 0 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.03 0.07 0.0075"/>
      </geometry>
      <material name="">
        <color rgba="0.9 0.0 0.0 1"/>
      </material>
    </collision>
  </link>
  <link name="right_inner_knuckle">
    <inertial>
      <origin rpy="0 0 0" xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166"/>
      <mass value="0.0271177346495152"/>
      <inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_inner_knuckle.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.1 0.1 0.1 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_inner_knuckle.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="finger_joint" type="revolute">
    <origin rpy="2.2957963267948966 0 0" xyz="0 -0.030601 0.054905"/>
    <parent link="robotiq_arg2f_base_link"/>
    <child link="left_outer_knuckle"/>
    <axis xyz="-1 0 0"/>
    <limit effort="1000" lower="0" upper="0.7" velocity="2.0"/>
  </joint>
  <joint name="left_outer_finger_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0.01821998610742 0.0260018192872234"/>
    <parent link="left_outer_knuckle"/>
    <child link="left_outer_finger"/>
    <axis xyz="1 0 0"/>
  </joint>
  <joint name="left_inner_knuckle_joint" type="revolute">
    <origin rpy="2.2957963267948966 0 0.0" xyz="0 -0.0127 0.06142"/>
    <parent link="robotiq_arg2f_base_link"/>
    <child link="left_inner_knuckle"/>
    <axis xyz="1 0 0"/>
    <limit effort="1000" upper="0" lower="-0.8757" velocity="2.0"/>
    <mimic joint="finger_joint" multiplier="-1" offset="0"/>
  </joint>
  <joint name="left_inner_finger_joint" type="revolute">
    <origin rpy="-0.725 0 0" xyz="0 0.0817554015893473 -0.0282203446692936"/>
    <parent link="left_outer_finger"/>
    <child link="left_inner_finger"/>
    <axis xyz="1 0 0"/>
    <limit effort="1000" lower="0" upper="0.8757" velocity="2.0"/>
    <mimic joint="finger_joint" multiplier="1" offset="0"/>
  </joint>
  <joint name="left_inner_finger_pad_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0.0457554015893473 -0.0272203446692936"/>
    <parent link="left_inner_finger"/>
    <child link="left_inner_finger_pad"/>
    <axis xyz="0 0 1"/>
  </joint>
  <joint name="right_outer_knuckle_joint" type="revolute">
    <origin rpy="2.2957963267948966 0 3.141592653589793" xyz="0 0.030601 0.054905"/>
    <parent link="robotiq_arg2f_base_link"/>
    <child link="right_outer_knuckle"/>
    <axis xyz="1 0 0"/>
    <limit effort="1000" upper="0" lower="-0.725" velocity="2.0"/>
    <mimic joint="finger_joint" multiplier="-1" offset="0"/>
  </joint>
  <joint name="right_outer_finger_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0.01821998610742 0.0260018192872234"/>
    <parent link="right_outer_knuckle"/>
    <child link="right_outer_finger"/>
    <axis xyz="1 0 0"/>
  </joint>
  <joint name="right_inner_knuckle_joint" type="revolute">
    <origin rpy="2.2957963267948966 0 -3.141592653589793" xyz="0 0.0127 0.06142"/>
    <parent link="robotiq_arg2f_base_link"/>
    <child link="right_inner_knuckle"/>
    <axis xyz="1 0 0"/>
    <limit effort="1000" upper="0" lower="-0.8757" velocity="2.0"/>
    <mimic joint="finger_joint" multiplier="-1" offset="0"/>
  </joint>
  <joint name="right_inner_finger_joint" type="revolute">
    <origin rpy="-0.725 0 0" xyz="0 0.0817554015893473 -0.0282203446692936"/>
    <parent link="right_outer_finger"/>
    <child link="right_inner_finger"/>
    <axis xyz="1 0 0"/>
    <limit effort="1000" lower="0" upper="0.8757" velocity="2.0"/>
    <mimic joint="finger_joint" multiplier="1" offset="0"/>
  </joint>
  <joint name="right_inner_finger_pad_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0.0457554015893473 -0.0272203446692936"/>
    <parent link="right_inner_finger"/>
    <child link="right_inner_finger_pad"/>
    <axis xyz="0 0 1"/>
  </joint>
<!--  <transmission name="finger_joint_trans">-->
<!--    <type>transmission_interface/SimpleTransmission</type>-->
<!--    <joint name="finger_joint">-->
<!--      <hardwareInterface>PositionJointInterface</hardwareInterface>-->
<!--    </joint>-->
<!--    <actuator name="finger_joint_motor">-->
<!--      <mechanicalReduction>1</mechanicalReduction>-->
<!--    </actuator>-->
<!--  </transmission>-->

<!--################################################
  ########          connect model                #####
  ####################################################-->

  <link name="world"/>
  <!-- connect gripper to arm -->
  <joint name="arm_gripper_joint" type="fixed">
    <parent link="ee_link"/>
    <child link="robotiq_arg2f_base_link"/>
    <origin rpy="-1.57 1.57 -1.57" xyz="0.0 0.0 0.0"/>
  </joint>

  <!-- connect arm to world -->
  <joint name="world_arm_joint" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
    <origin rpy="0.0 0.0 0" xyz="0 0 0"/>
  </joint>


</robot>

