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

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

    <!-- Joint connecting the parent to our frame_in -->
    <joint name="${prefix}adapter_iso50_kms40_joint" type="fixed">
      <xacro:insert_block name="origin"/>
      <parent link="${parent}"/>
      <child link="${prefix}adapter_iso50_kms40_frame_in"/>
    </joint>

    <!-- link for the adapter plate -->
    <link name="${prefix}adapter_iso50_kms40_frame_in">
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_kms_40_description/meshes/adapter_plate_iso50_kms40_weiss.stl" scale="0.001 0.001 0.001"/>
        </geometry>
        <material name="Grey"/>
      </visual>
    </link>


    <!-- Empty link just to get the frame out -->
    <link name="${prefix}adapter_iso50_kms40_frame_out"/>


    <!-- joint positioning the frame_out -->
    <joint name="${prefix}adapter_iso50_kms40_trans_frame_out" type="fixed">
      <origin xyz="0 0 0.016" rpy="0 0 0" />
      <parent link="${prefix}adapter_iso50_kms40_frame_in" />
      <child link="${prefix}adapter_iso50_kms40_frame_out" />
      <axis xyz="0 0 0" />
    </joint>


    <!-- Now joint connecting the actual kms40 sensor -->


    <joint name="${prefix}kms40_joint" type="fixed">
      <origin xyz="0 0 0" rpy="0 0 0" />
      <parent link="${prefix}adapter_iso50_kms40_frame_out"/>
      <child link="${prefix}kms40_frame_in"/>
    </joint>

    <!-- Link for the kms40 sensor-->
    <link name="${prefix}kms40_frame_in">
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_kms_40_description/meshes/kms40_simple.stl" scale="0.001 0.001 0.001"/>
        </geometry>
        <material name="Grey"/>
      </visual>
    </link>

    <!-- Empty link just to get the frame out -->
    <!-- This is also the reference frame for the force/torque data -->
    <link name="${prefix}kms40_frame_out"/>


    <!-- joint positioning the frame_out -->
    <joint name="${prefix}kms40_trans_frame_out" type="fixed">
      <origin xyz="0 0 0.027" rpy="0 0 0" />
      <parent link="${prefix}kms40_frame_in" />
      <child link="${prefix}kms40_frame_out" />
      <axis xyz="0 0 0" />
    </joint>


  </xacro:macro>
</robot>
