<?xml version="1.0"?>
<robot name="ur5" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:include filename="$(find ur5_description)/urdf/ur5_joint_limited_robot.urdf.xacro" />
  <xacro:include filename="$(find robotiq_description)/urdf/robotiq_85_gripper.urdf.xacro" />
  
  <!-- Robotiq Coupler -->
  <!--  + Height added by the coupler: 8mm -->
  <!--  + Reference frame: at the middle (4mm) -->
  <link name="robotiq_coupler">
    <visual>
      <geometry>
        <mesh filename="package://robotiq_description/meshes/robotiq_85_coupler.stl" />
      </geometry>
      <material name="flat_black"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://robotiq_description/meshes/robotiq_85_coupler.stl" />
      </geometry>
    </collision>
    <inertial>
      <origin xyz="2.073e-05   1.45286e-03  -1.1049e-03" rpy="0 0 0" />
      <mass value="0.168" />
      <inertia ixx="6.69695624e-05" ixy="5.21511788e-09" ixz="-2.74383009e-08" 
               iyy="7.85088161e-05" iyz="5.41105193e-07" izz="1.41819717e-04"/>
    </inertial>
  </link>

  <joint name="robotiq_coupler_joint" type="fixed">
    <origin xyz="0 0 0.004" rpy="0 0 ${-pi/2.0}" />
    <parent link="tool0"/>
    <child link="robotiq_coupler"/>
  </joint>
  <gazebo reference="robotiq_coupler">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
    <material>Gazebo/FlatBlack</material>
  </gazebo>
  
  <!-- Equivalent to the OpenRAVE manipulator denso_robotiq_85_gripper -->
  <!-- <link name="denso_robotiq_85_gripper" /> -->
  <!-- <joint name="manipulator_dummy_joint" type="fixed"> -->
  <!--   <origin xyz="0 0 0.1441" rpy="0 0 0" /> -->
  <!--   <parent link="robotiq_coupler"/> -->
  <!--   <child link="denso_robotiq_85_gripper"/> -->
  <!-- </joint> -->
  
  <!-- Attach the robotiq 85 gripper -->
  <xacro:robotiq_85_gripper prefix="" parent="robotiq_coupler" >
    <origin xyz="0 0 0.004" rpy="0 ${-pi/2} ${pi}"/>
  </xacro:robotiq_85_gripper> 
  
  <!-- Gazebo FT sensor plugin -->
  <gazebo reference="wrist_3_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <gazebo>
    <plugin name="ft_sensor_plugin" filename="libgazebo_ros_ft_sensor.so">
      <updateRate>250.0</updateRate>
      <topicName>ft_sensor/raw</topicName>
      <gaussianNoise>0.0</gaussianNoise>
      <jointName>wrist_3_joint</jointName>
    </plugin>
  </gazebo>
  
  <!-- Gazebo grasping plugin -->
  <gazebo>
    <gripper name="gazebo_gripper">
      <grasp_check>
        <attach_steps>2</attach_steps>    <!-- default: 20 -->
        <detach_steps>2</detach_steps>    <!-- default: 40 -->
        <min_contact_count>3</min_contact_count>
      </grasp_check>
      <gripper_link>robotiq_85_left_finger_tip_link</gripper_link>
      <gripper_link>robotiq_85_right_finger_tip_link</gripper_link>
      <palm_link>robotiq_85_base_link</palm_link>
    </gripper>
  </gazebo>
  
</robot>
