<?xml version="1.0"?>
<robot  xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
	xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
	xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
        xmlns:xacro="http://www.ros.org/wiki/xacro"
	name="wsg_50">

<xacro:macro name="wsg_50_xacro" params="name parent *origin">

 <joint name="${name}_anterior_gripper_joint" type="fixed">
    <xacro:insert_block name="origin"/>
    <parent link="${parent}"/>
    <child link="${name}_base_link"/>
 </joint>

<!-- BASE LINK -->
 <link name="${name}_base_link">
      <inertial>
          <mass value="0.25" />
          <origin xyz="0 0 0" />
     	  <!--inertia ixx="1.0" ixy="0.0" ixz="0.0"
               iyy="100.0" iyz="0.0" 
               izz="1.0" /-->
      	  <inertia  ixx="1.0" ixy="0.0"  ixz="0.0"  iyy="1.0"  iyz="0.0"  izz="1.0" />
      </inertial>

      <visual>
          <origin xyz="0.0 0 0" rpy="0 0 0" />
          <geometry>
             <mesh filename="package://iai_wsg_50_description/meshes/WSG50_110.stl" scale="1 1 1"/>
	     <!--box size="0.146 0.05 0.0735"/-->
          </geometry>
          <material name="grey">
		<color rgba="0.5 0.5 0.5 1"/>
      	  </material>
      </visual>

      <collision>
          <origin xyz="0.0 0 0" rpy="0 0 0" />
          <geometry>
	     <!--box size="0.146 0.05 0.0735"/-->
             <mesh filename="package://iai_wsg_50_description/meshes/WSG50_110.stl" scale="1 1 1"/>
         </geometry>
     </collision>
  </link>

  <gazebo reference="${name}_base_link">
    <material>Gazebo/Grey</material>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>

  <!-- GRIPPER LEFT -->

  <joint name="${name}_base_gripper_left_joint" type="prismatic">
     <mimic joint ="${name}_joint" multiplier="-0.5"/>
     <limit lower="-0.055" upper="-0.0027" effort="1.0" velocity="0.42"/>
     <origin xyz="0 0 0" rpy="0 0 0" />      <!--origin xyz="-0.0067 0 0.049" rpy="0 0 0" /-->
     <parent link="${name}_base_link"/>
     <child link="${name}_gripper_left_link" />
     <dynamics friction="100" damping="100" />
     <axis xyz="1 0 0"/>
     <!-- <limit effort="100" velocity="100"/> -->
  </joint>

  <transmission name="${name}_base_trans_left" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="${name}_base_motor_left" />
     <joint name="${name}_base_gripper_left_joint" />
     <mechanicalReduction>1</mechanicalReduction>
     <motorTorqueConstant>1</motorTorqueConstant>
  </transmission>

  <link name="${name}_gripper_left_link">
      <inertial>
          <mass value="0.1" />
          <origin xyz="0 0 0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
               iyy="1.0" iyz="0.0" 
               izz="1.0" />
      </inertial>

      <visual>
          <origin xyz="0.0 0 0" rpy="0 0 0" />
          <geometry>
             <mesh filename="package://iai_wsg_50_description/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
	     <!--box size="0.0175 0.03 0.024"/-->
          </geometry>
          <material name="black">
		<color rgba="0 0 0 1"/>
      	  </material>
      </visual>

      <collision>
          <origin xyz="0 0 0" rpy="0 0 0" />
          <geometry>
	     <!--box size="0.0175 0.03 0.024"/-->
             <mesh filename="package://iai_wsg_50_description/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
         </geometry>
     </collision>
     <contact_coefficients kd="1.0" kp="1000.0" mu="0"/>
  </link>

  <gazebo reference="${name}_gripper_left_link">
    <material>Gazebo/Blue</material>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>


  <!-- LEFT FINGER -->

  <joint name="${name}_guide_joint_finger_left" type="fixed">
     <origin xyz="0 0 0.023" rpy="0 0 0" />
     <parent link="${name}_gripper_left_link"/>
     <child link="${name}_finger_left_link" />
     <dynamics friction="100" damping="100" />
     <axis xyz="1 0 0"/>
     <!-- <limit effort="100" velocity="100"/> -->
  </joint>

  <link name="${name}_finger_left_link">
      <inertial>
          <mass value="0.1" />
          <origin xyz="0 0 0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
               iyy="1.0" iyz="0.0" 
               izz="1.0" />
      </inertial>

      <visual>
          <origin xyz="0.0 0 0" rpy="0 0 0" />
          <geometry>
             <mesh filename="package://iai_wsg_50_description/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
          </geometry>
          <material name="black">
		<color rgba="0 0 0 1"/>
      	  </material>
      </visual>

      <collision>
          <origin xyz="0 0 0" rpy="0 0 0" />
          <geometry>
             <mesh filename="package://iai_wsg_50_description/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
         </geometry>
     </collision>
     <contact_coefficients kd="1.0" kp="1000.0" mu="0"/>
  </link>

  <gazebo reference="${name}_finger_left">
    <material>Gazebo/Blue</material>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>


  <!-- GRIPPER RIGHT -->

  <joint name="${name}_joint" type="prismatic">
     <limit lower="0.0065" upper="0.109" effort="1.0" velocity="0.42"/>
     <origin xyz="0 0 0" rpy="0 0 3.14159" />
     <parent link="${name}_gripper_left_link"/>
     <child link="${name}_gripper_right_link" />
     <axis xyz="-1 0 0"/>
     <dynamics friction="100" damping="100" />
     <!-- <limit effort="100" velocity="100"/>-->
  </joint>

  <transmission name="${name}_base_trans_right" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="${name}_base_motor_right" />
     <joint name="${name}_base_gripper_right_joint" />
     <mechanicalReduction>1</mechanicalReduction>
     <motorTorqueConstant>1</motorTorqueConstant>
  </transmission>
	
    <link name="${name}_gripper_right_link">
      <inertial>
          <mass value="0.1" />
          <origin xyz="0 0 0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
               iyy="1.0" iyz="0.0" 
               izz="1.0" />
      </inertial>

      <visual>
          <origin xyz="0.0 0 0" rpy="0 0 0" />
          <geometry>
             <mesh filename="package://iai_wsg_50_description/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
	     <!--box size="0.0175 0.03 0.024"/-->
          </geometry>
          <material name="black">
		<color rgba="0 0 0 1"/>
      	  </material>
      </visual>

      <collision>
          <origin xyz="0 0 0" rpy="0 0 0" />
          <geometry>
	     <!--box size="0.0175 0.03 0.024"/-->
             <mesh filename="package://iai_wsg_50_description/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
         </geometry>
     </collision>
     <contact_coefficients kd="1.0" kp="1000.0" mu="0"/>
  </link>

  <gazebo reference="gripper_right">
    <material>Gazebo/White</material>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>

  <!-- RIGHT FINGER -->

  <joint name="${name}_guide_joint_finger_right" type="fixed">
     <origin xyz="0 0 0.023" rpy="0 0 0" />
     <parent link="${name}_gripper_right_link"/>
     <child link="${name}_finger_right_link" />
  </joint>

  <link name="${name}_finger_right_link">
      <inertial>
          <mass value="0.1" />
          <origin xyz="0 0 0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
               iyy="1.0" iyz="0.0" 
               izz="1.0" />
      </inertial>

      <visual>
          <origin xyz="0.0 0 0" rpy="0 0 0" />
          <geometry>
             <mesh filename="package://iai_wsg_50_description/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
          </geometry>
          <material name="black">
		<color rgba="0 0 0 1"/>
      	  </material>
      </visual>

      <collision>
          <origin xyz="0 0 0" rpy="0 0 0" />
          <geometry>
             <mesh filename="package://iai_wsg_50_description/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
         </geometry>
     </collision>
     <contact_coefficients kd="1.0" kp="1000.0" mu="0"/>
  </link>

  <gazebo reference="${name}_finger_right">
    <material>Gazebo/White</material>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>


  <link name="${name}_tool_frame"/>

  <joint name="${name}_tool_frame_joint" type="fixed">
     <origin xyz="0 0 0.144" rpy="0 0 0" />
     <parent link="${name}_base_link"/>
     <child link="${name}_tool_frame" />
  </joint>

</xacro:macro>

</robot>

