<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from /home/stelter/workspace20/iai_ws/src/iai_kmr_iiwa/iai_kmr_iiwa_description/urdf/kmr_iiwa.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="kmr_iiwa" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor">
  <material name="Black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>
  <material name="Blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>
  <material name="Green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>
  <material name="Grey">
    <color rgba="0.4 0.4 0.4 1.0"/>
  </material>
  <material name="Orange">
    <color rgba="1.0 0.4235294117647059 0.0392156862745098 1.0"/>
  </material>
  <material name="Brown">
    <color rgba="0.8705882352941177 0.8117647058823529 0.7647058823529411 1.0"/>
  </material>
  <material name="Red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  </material>
  <material name="White">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>
  <!-- Fix to world just for testing -->
  <material name="gray">
    <color rgba="0.7 0.7 0.7 1.0"/>
  </material>
  <!-- <base/> -->
  <!--   <link name="base_footprint"/>

    <joint name="base_joint" type="fixed">
      <parent link="base_footprint"/>
      <child link="base_link"/>
      <origin xyz="0 0 0" rpy="0 0 0"/>
    </joint> -->
  <link name="base_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_kmr_iiwa_description/meshes/kmp200_3ds_export.dae"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_kmr_iiwa_description/meshes/kmp200_3ds_export_s.stl"/>
      </geometry>
      <material name="Grey"/>
    </collision>
  </link>
  <joint name="back_laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="back_laser"/>
    <origin rpy="0.000 -0.000 2.356" xyz="-0.464 0.256 0.140"/>
  </joint>
  <link name="back_laser"/>
  <joint name="front_laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="front_laser"/>
    <origin rpy="0.000 0.000 -0.785" xyz="0.464 -0.256 0.140"/>
  </joint>
  <link name="front_laser"/>
  <joint name="angle_adapter_base_joint" type="fixed">
    <parent link="base_link"/>
    <child link="angle_adapter_base"/>
    <origin rpy="0 0 1.5707963267948966" xyz="0.363 0 0.7"/>
  </joint>
  <link name="angle_adapter_base">
    <visual>
      <geometry>
        <mesh filename="package://iai_kmr_iiwa_description/meshes/angle_adapter_base.stl" scale="1 1 1"/>
      </geometry>
      <material name="gray"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://iai_kmr_iiwa_description/meshes/angle_adapter_base.stl"/>
      </geometry>
      <material name="gray"/>
    </collision>
  </link>
  <joint name="angle_adapter_connector_joint" type="fixed">
    <parent link="angle_adapter_base"/>
    <child link="angle_adapter_connector"/>
    <origin rpy="0.7853981633974483 0 0" xyz="0 -0.100197 0.0747412"/>
    <!-- <axis xyz="1 0 0"/> -->
    <!-- <limit effort="150.0" lower="0" upper="${pi/2}" velocity="1"/> -->
  </joint>
  <link name="angle_adapter_connector"/>
  <joint name="angle_adapter_plate_joint" type="fixed">
    <parent link="angle_adapter_connector"/>
    <child link="angle_adapter_plate"/>
    <origin rpy="0 0 0" xyz="0 0.1 0.00"/>
  </joint>
  <link name="angle_adapter_plate">
    <visual>
      <geometry>
        <mesh filename="package://iai_kmr_iiwa_description/meshes/angle_adapter_plate.stl" scale="1 1 1"/>
      </geometry>
      <material name="gray"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://iai_kmr_iiwa_description/meshes/angle_adapter_plate.stl" scale="1 1 1"/>
      </geometry>
      <material name="gray"/>
    </collision>
  </link>
  <!--joint between {parent} and link_0-->
  <joint name="angle_adapter_plate_iiwa_joint" type="fixed">
    <origin rpy="0 0 -1.5707963267948966" xyz="0 0 0.00"/>
    <parent link="angle_adapter_plate"/>
    <child link="iiwa_link_0"/>
  </joint>
  <link name="iiwa_link_0">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.1 0 0.07"/>
      <mass value="5"/>
      <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.06" iyz="0" izz="0.03"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/visual/link_0.stl"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/collision/link_0.stl"/>
      </geometry>
      <material name="Grey"/>
    </collision>
    <self_collision_checking>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <capsule length="0.25" radius="0.15"/>
      </geometry>
    </self_collision_checking>
  </link>
  <!-- joint between link_0 and link_1 -->
  <joint name="iiwa_joint_1" type="revolute">
    <parent link="iiwa_link_0"/>
    <child link="iiwa_link_1"/>
    <origin rpy="0 0 0" xyz="0 0 0.1575"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.9670597283903604" upper="2.9670597283903604" velocity="10"/>
    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.9321531433504737" soft_upper_limit="2.9321531433504737"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="iiwa_link_1">
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
      <mass value="4"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/visual/link_1.stl"/>
      </geometry>
      <material name="Orange"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/collision/link_1.stl"/>
      </geometry>
      <material name="Orange"/>
    </collision>
  </link>
  <!-- joint between link_1 and link_2 -->
  <joint name="iiwa_joint_2" type="revolute">
    <parent link="iiwa_link_1"/>
    <child link="iiwa_link_2"/>
    <origin rpy="1.5707963267948966   0 3.141592653589793" xyz="0 0 0.2025"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="10"/>
    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.0594885173533086" soft_upper_limit="2.0594885173533086"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="iiwa_link_2">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0003 0.059 0.042"/>
      <mass value="4"/>
      <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.044"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/visual/link_2.stl"/>
      </geometry>
      <material name="Orange"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/collision/link_2.stl"/>
      </geometry>
      <material name="Orange"/>
    </collision>
  </link>
  <!-- joint between link_2 and link_3 -->
  <joint name="iiwa_joint_3" type="revolute">
    <parent link="iiwa_link_2"/>
    <child link="iiwa_link_3"/>
    <origin rpy="1.5707963267948966 0 3.141592653589793" xyz="0 0.2045 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.9670597283903604" upper="2.9670597283903604" velocity="10"/>
    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.9321531433504737" soft_upper_limit="2.9321531433504737"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="iiwa_link_3">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0.03 0.13"/>
      <mass value="3"/>
      <inertia ixx="0.08" ixy="0" ixz="0" iyy="0.075" iyz="0" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/visual/link_3.stl"/>
      </geometry>
      <material name="Orange"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/collision/link_3.stl"/>
      </geometry>
      <material name="Orange"/>
    </collision>
  </link>
  <!-- joint between link_3 and link_4 -->
  <joint name="iiwa_joint_4" type="revolute">
    <parent link="iiwa_link_3"/>
    <child link="iiwa_link_4"/>
    <origin rpy="1.5707963267948966 0 0" xyz="0 0 0.2155"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="10"/>
    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.0594885173533086" soft_upper_limit="2.0594885173533086"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="iiwa_link_4">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0.067 0.034"/>
      <mass value="2.7"/>
      <inertia ixx="0.03" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.029"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/visual/link_4.stl"/>
      </geometry>
      <material name="Orange"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/collision/link_4.stl"/>
      </geometry>
      <material name="Orange"/>
    </collision>
  </link>
  <!-- joint between link_4 and link_5 -->
  <joint name="iiwa_joint_5" type="revolute">
    <parent link="iiwa_link_4"/>
    <child link="iiwa_link_5"/>
    <origin rpy="-1.5707963267948966 3.141592653589793 0" xyz="0 0.1845 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.9670597283903604" upper="2.9670597283903604" velocity="10"/>
    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.9321531433504737" soft_upper_limit="2.9321531433504737"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="iiwa_link_5">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0001 0.021 0.076"/>
      <mass value="1.7"/>
      <inertia ixx="0.02" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.005"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/visual/link_5.stl"/>
      </geometry>
      <material name="Orange"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/collision/link_5.stl"/>
      </geometry>
      <material name="Orange"/>
    </collision>
  </link>
  <!-- joint between link_5 and link_6 -->
  <joint name="iiwa_joint_6" type="revolute">
    <parent link="iiwa_link_5"/>
    <child link="iiwa_link_6"/>
    <origin rpy="1.5707963267948966 0 0" xyz="0 0 0.2155"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="10"/>
    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.0594885173533086" soft_upper_limit="2.0594885173533086"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="iiwa_link_6">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0.0006 0.0004"/>
      <mass value="1.8"/>
      <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.0036" iyz="0" izz="0.0047"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/visual/link_6.stl"/>
      </geometry>
      <material name="Orange"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/collision/link_6.stl"/>
      </geometry>
      <material name="Orange"/>
    </collision>
  </link>
  <!-- joint between link_6 and link_7 -->
  <joint name="iiwa_joint_7" type="revolute">
    <parent link="iiwa_link_6"/>
    <child link="iiwa_link_7"/>
    <origin rpy="-1.5707963267948966 3.141592653589793 0" xyz="0 0.081 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-3.0543261909900763" upper="3.0543261909900763" velocity="10"/>
    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-3.01941960595019" soft_upper_limit="3.01941960595019"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="iiwa_link_7">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0.02"/>
      <mass value="0.3"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/visual/link_7-MF-Touch-pneumatisch.stl"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://iai_iiwa_description/meshes/iiwa14/collision/link_7-MF-Touch-pneumatisch.stl"/>
      </geometry>
      <material name="Grey"/>
    </collision>
  </link>
  <joint name="iiwa_joint_ee" type="fixed">
    <parent link="iiwa_link_7"/>
    <child link="iiwa_link_ee"/>
    <origin rpy="0 0 0" xyz="0 0 0.045"/>
  </joint>
  <link name="iiwa_link_ee">
    </link>
  <!-- Enable the Joint Feedback -->
  <gazebo reference="iiwa_joint_7">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <!-- The ft_sensor plugin -->
  <gazebo>
    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
      <updateRate>50.0</updateRate>
      <topicName>/iiwa/state/CartesianWrench</topicName>
      <jointName>iiwa_joint_7</jointName>
    </plugin>
  </gazebo>
  <!-- Load Gazebo lib and set the robot namespace -->
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <robotNamespace>/iiwa</robotNamespace>
    </plugin>
  </gazebo>
  <!-- Link0 -->
  <gazebo reference="iiwa_link_0">
    <material>Gazebo/Grey</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <!-- Link1 -->
  <gazebo reference="iiwa_link_1">
    <material>Gazebo/Orange</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <!-- Link2 -->
  <gazebo reference="iiwa_link_2">
    <material>Gazebo/Orange</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <!-- Link3 -->
  <gazebo reference="iiwa_link_3">
    <material>Gazebo/Orange</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <!-- Link4 -->
  <gazebo reference="iiwa_link_4">
    <material>Gazebo/Orange</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <!-- Link5 -->
  <gazebo reference="iiwa_link_5">
    <material>Gazebo/Orange</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <!-- Link6 -->
  <gazebo reference="iiwa_link_6">
    <material>Gazebo/Orange</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <!-- Link7 -->
  <gazebo reference="iiwa_link_7">
    <material>Gazebo/Grey</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <transmission name="iiwa_tran_1">
    <robotNamespace>/iiwa</robotNamespace>
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="iiwa_joint_1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="iiwa_motor_1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="iiwa_tran_2">
    <robotNamespace>/iiwa</robotNamespace>
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="iiwa_joint_2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="iiwa_motor_2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="iiwa_tran_3">
    <robotNamespace>/iiwa</robotNamespace>
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="iiwa_joint_3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="iiwa_motor_3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="iiwa_tran_4">
    <robotNamespace>/iiwa</robotNamespace>
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="iiwa_joint_4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="iiwa_motor_4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="iiwa_tran_5">
    <robotNamespace>/iiwa</robotNamespace>
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="iiwa_joint_5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="iiwa_motor_5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="iiwa_tran_6">
    <robotNamespace>/iiwa</robotNamespace>
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="iiwa_joint_6">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="iiwa_motor_6">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="iiwa_tran_7">
    <robotNamespace>/iiwa</robotNamespace>
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="iiwa_joint_7">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="iiwa_motor_7">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!--    <link name="kms_mounting_plate">
      <visual>
        <geometry>
          <mesh filename="package://iai_table_robot_description/meshes/adapter_plate_iso50_kms40_weiss.stl" scale="0.001 0.001 0.001"/>
        </geometry>
        <material name="gray"/>
      </visual>
    </link>

    <joint name="kms_mounting_plate_joint" type="fixed">
      <parent link="iiwa_link_ee" />
      <child link="kms_mounting_plate" />
      <origin xyz="0 0 0" rpy="0 0 0" />
    </joint>

    <xacro:kms_40_xacro parent="kms_mounting_plate" prefix="">
      <origin xyz="0 0 0" rpy="0 0 ${pi}" />
    </xacro:kms_40_xacro> -->
  <!-- 
    <xacro:adapter_kms40_fwk050_xacro parent="kms40_frame_out" prefix="">
      <origin xyz="0 0 0" rpy="0 0 0" /> 
    </xacro:adapter_kms40_fwk050_xacro> -->
  <!-- Joint connecting the parent to our frame_in -->
  <joint name="fwk_fwa_050_in_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.026"/>
    <parent link="iiwa_link_ee"/>
    <child link="fwk_fwa_050_frame_in"/>
  </joint>
  <link name="fwk_fwa_050_frame_in">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <!-- these meshes were exported in mm, so scale back to m -->
        <mesh filename="package://iai_adapters_description/meshes/fwk_fwa_050_simple.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <!-- these meshes were exported in mm, so scale back to m -->
        <mesh filename="package://iai_adapters_description/meshes/fwk_fwa_050_simple.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="Grey"/>
    </collision>
  </link>
  <!-- Empty link just to get the frame out -->
  <link name="fwk_fwa_050_frame_out"/>
  <!-- joint positioning the frame_out -->
  <joint name="fwk_fwa_050_trans_frame_out" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.0141"/>
    <parent link="fwk_fwa_050_frame_in"/>
    <child link="fwk_fwa_050_frame_out"/>
    <axis xyz="0 0 0"/>
  </joint>
  <!-- The joint connecting us to the next part of the chain is in the next URDF -->
  <!--     <xacro:adapter_fwa050_wsg50_xacro parent="fwk_fwa_050_frame_out" prefix="">
      <origin xyz="0 0 0.0" rpy="0 0 0" />
    </xacro:adapter_fwa050_wsg50_xacro> -->
  <joint name="gripper_anterior_gripper_joint" type="fixed">
    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
    <parent link="fwk_fwa_050_frame_out"/>
    <child link="gripper_base_link"/>
  </joint>
  <!-- BASE LINK -->
  <link name="gripper_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 rpy="0 0 0" xyz="0.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 rpy="0 0 0" xyz="0.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="gripper_base_link">
    <material>Gazebo/Grey</material>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <!-- GRIPPER LEFT -->
  <joint name="gripper_base_gripper_left_joint" type="prismatic">
    <mimic joint="gripper_joint" multiplier="-0.5"/>
    <limit effort="1.0" lower="-0.055" upper="-0.0027" velocity="0.42"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <!--origin xyz="-0.0067 0 0.049" rpy="0 0 0" /-->
    <parent link="gripper_base_link"/>
    <child link="gripper_gripper_left_link"/>
    <dynamics damping="100" friction="100"/>
    <axis xyz="1 0 0"/>
    <!-- <limit effort="100" velocity="100"/> -->
  </joint>
  <transmission name="gripper_base_trans_left" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="gripper_base_motor_left"/>
    <joint name="gripper_base_gripper_left_joint"/>
    <mechanicalReduction>1</mechanicalReduction>
    <motorTorqueConstant>1</motorTorqueConstant>
  </transmission>
  <link name="gripper_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 rpy="0 0 0" xyz="0.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 rpy="0 0 0" xyz="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_gripper_left_link">
    <material>Gazebo/Blue</material>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <!-- LEFT FINGER -->
  <joint name="gripper_guide_joint_finger_left" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.023"/>
    <parent link="gripper_gripper_left_link"/>
    <child link="gripper_finger_left_link"/>
    <dynamics damping="100" friction="100"/>
    <axis xyz="1 0 0"/>
    <!-- <limit effort="100" velocity="100"/> -->
  </joint>
  <link name="gripper_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 rpy="0 0 0" xyz="0.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 rpy="0 0 0" xyz="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="gripper_finger_left">
    <material>Gazebo/Blue</material>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <!-- GRIPPER RIGHT -->
  <joint name="gripper_joint" type="prismatic">
    <limit effort="1.0" lower="0.0065" upper="0.109" velocity="0.42"/>
    <origin rpy="0 0 3.14159" xyz="0 0 0"/>
    <parent link="gripper_gripper_left_link"/>
    <child link="gripper_gripper_right_link"/>
    <axis xyz="-1 0 0"/>
    <dynamics damping="100" friction="100"/>
    <!-- <limit effort="100" velocity="100"/>-->
  </joint>
  <transmission name="gripper_base_trans_right" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="gripper_base_motor_right"/>
    <joint name="gripper_base_gripper_right_joint"/>
    <mechanicalReduction>1</mechanicalReduction>
    <motorTorqueConstant>1</motorTorqueConstant>
  </transmission>
  <link name="gripper_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 rpy="0 0 0" xyz="0.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 rpy="0 0 0" xyz="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="gripper_guide_joint_finger_right" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.023"/>
    <parent link="gripper_gripper_right_link"/>
    <child link="gripper_finger_right_link"/>
  </joint>
  <link name="gripper_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 rpy="0 0 0" xyz="0.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 rpy="0 0 0" xyz="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="gripper_finger_right">
    <material>Gazebo/White</material>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="gripper_tool_frame"/>
  <joint name="gripper_tool_frame_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.144"/>
    <parent link="gripper_base_link"/>
    <child link="gripper_tool_frame"/>
  </joint>
  <link name="camera_holder_link">
    <visual>
      <geometry>
        <mesh filename="package://iai_donbot_description/meshes/refills_camera_holder_and_camera.stl" scale="1 1 1"/>
      </geometry>
      <material name="gray"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://iai_donbot_description/meshes/refills_camera_holder_and_camera.stl" scale="1 1 1"/>
      </geometry>
      <material name="gray"/>
    </collision>
  </link>
  <joint name="gripper_to_camera_holder_joint" type="fixed">
    <parent link="gripper_base_link"/>
    <child link="camera_holder_link"/>
    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
  </joint>
  <link name="camera_link"/>
  <joint name="iiwa_link_ee_to_camera_joint" type="fixed">
    <parent link="iiwa_link_ee"/>
    <child link="camera_link"/>
    <!--Calib_errors = [0.003425, 0.2069, 0.00611, 0.42678]
          Calib_errors = median_pos_error(m), median_orient_error(deg), max_pos_error(m), max_orient_error(deg) -->
    <!-- from base_link origin xyz="0.004174 -0.067273 0.062438" rpy="0.209203 0.001106 3.120208"/-->
    <!-- from ur5_base_link origin xyz="0.002956 -0.066498 0.060947" rpy="0.204429 6.281375 3.127197"/-->
    <!-- calib errors: 0.00311, 0.24373, 0.00688, 0.4558 -->
    <!-- what we calibrated with alexis, relative to ee link -->
    <!-- <origin xyz="0.001329 -0.066190 0.111783" rpy="0.231224 6.279694 3.148548"/>  -->
    <!-- from tf lookup shifted to gripper_base_link -->
    <origin rpy="0.231224 -0.00349130717959 0.00695534641" xyz="-0.00132899999999 0.06619 0.071683"/>
  </joint>
  <link name="rs_camera_link"/>
  <joint name="wristcam_to_realsense_joint" type="fixed">
    <parent link="camera_link"/>
    <child link="rs_camera_link"/>
    <!-- <origin xyz="-0.017993 -0.107556 0.017699" rpy="6.270618 4.930698 1.578454"/> -->
    <origin rpy="6.271141 4.940698 1.579879" xyz="-0.018594 -0.105693 0.017161"/>
  </joint>
  <joint name="wrist_collision_joint" type="fixed">
    <parent link="iiwa_link_ee"/>
    <child link="wrist_collision"/>
    <origin rpy="0 0 0" xyz="0 0 -0.005"/>
  </joint>
  <link name="wrist_collision">
    <collision>
      <geometry>
        <cylinder length="0.08" radius="0.04"/>
      </geometry>
      <material name="Silver"/>
    </collision>
  </link>
  <joint name="refills_tool_frame_joint" type="fixed">
    <parent link="gripper_tool_frame"/>
    <child link="refills_tool_frame"/>
    <origin rpy="0.0 0.0 0.0" xyz="0 0 0.037"/>
  </joint>
  <link name="refills_tool_frame"/>
  <joint name="refills_finger_joint" type="continuous">
    <parent link="refills_tool_frame"/>
    <child link="refills_finger"/>
    <axis xyz="1 0 0"/>
  </joint>
  <link name="refills_finger"/>
</robot>


