<?xml version="1.0"?>

<robot name="iai_drawers" xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:property name="pi" value="3.1415926535897931" />

  
  
  <macro name="iai_drawer_60_14" params="name parent *origin">   
    <iai_drawer_14 name="${name}" parent="${parent}" width="60">
      <xacro:insert_block name="origin" />
    </iai_drawer_14>
  </macro>
  
  <macro name="iai_drawer_80_14" params="name parent *origin">   
    <iai_drawer_14 name="${name}" parent="${parent}" width="80">
      <xacro:insert_block name="origin" />
    </iai_drawer_14>
  </macro>
  
  <macro name="iai_drawer_60_29" params="name parent *origin">   
    <iai_drawer_29 name="${name}" parent="${parent}" width="60">
      <xacro:insert_block name="origin" />
    </iai_drawer_29>
  </macro>
  
  <macro name="iai_drawer_80_29" params="name parent *origin">   
    <iai_drawer_29 name="${name}" parent="${parent}" width="80">
      <xacro:insert_block name="origin" />
    </iai_drawer_29>
  </macro>
  
  
  <macro name="iai_drawer_100_29" params="name parent *origin">   
    <iai_drawer_29 name="${name}" parent="${parent}" width="100">
      <xacro:insert_block name="origin" />
    </iai_drawer_29>
  </macro>
  
  
  <macro name="iai_drawer_60_58" params="name parent *origin">   
    <iai_drawer_58 name="${name}" parent="${parent}" width="60">
      <xacro:insert_block name="origin" />
    </iai_drawer_58>
  </macro>
  
  <macro name="iai_drawer_58" params="name parent width *origin">   
    
    <iai_generic_drawer name="${name}" parent="${parent}" width="${width}" height="58">
      <xacro:insert_block name="origin" />
      <origin xyz="0.37 0 0.25" rpy="0 0 0" />
    </iai_generic_drawer>
  </macro>


  <xacro:property name="board_x" value="0.49"/>
  <xacro:property name="board_y" value="0.25"/>
  <xacro:property name="board_z" value="0.016"/>
  <xacro:property name="barrier_z" value="0.082"/>
  <macro name="iai_vdrawer_board" params="name parent joint_origin_z">
    <link name="${parent}_board_${name}_link">
      <sphere_inertia radius="0" mass="0"/>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="${board_x} ${board_y} ${board_z}"/>
        </geometry>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="${board_x} ${board_y} ${board_z}"/>
        </geometry>
      </collision>
    </link>

    <joint name="${parent}_board_${name}_joint" type="fixed">
      <origin xyz="${-board_y} 0 ${joint_origin_z}"/>
      <parent link="${parent}_main"/>
      <child link="${parent}_board_${name}_link"/>
    </joint>

    <link name="${parent}_barrier_${name}_right_link">
      <sphere_inertia radius="0" mass="0"/>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="${board_x} ${board_z} ${barrier_z}"/>
        </geometry>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="${board_x} ${board_z} ${barrier_z}"/>
        </geometry>
      </collision>
    </link>

    <joint name="${parent}_barrier_${name}_right_joint" type="fixed">
      <origin xyz="${-board_y} ${-board_y / 2 + board_z / 2} ${joint_origin_z + barrier_z/2 - board_z/2}"/>
      <parent link="${parent}_main"/>
      <child link="${parent}_barrier_${name}_right_link"/>
    </joint>

    <link name="${parent}_barrier_${name}_left_link">
      <sphere_inertia radius="0" mass="0"/>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="${board_x} ${board_z} ${barrier_z}"/>
        </geometry>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="${board_x} ${board_z} ${barrier_z}"/>
        </geometry>
      </collision>
    </link>

    <joint name="${parent}_barrier_${name}_left_joint" type="fixed">
      <origin xyz="${-board_y} ${board_y / 2 - board_z / 2} ${joint_origin_z + barrier_z/2 - board_z/2}"/>
      <parent link="${parent}_main"/>
      <child link="${parent}_barrier_${name}_left_link"/>
    </joint>

    <link name="${parent}_barrier_${name}_back_link">
      <sphere_inertia radius="0" mass="0"/>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="${board_z} ${board_y} ${barrier_z}"/>
        </geometry>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="${board_z} ${board_y} ${barrier_z}"/>
        </geometry>
      </collision>
    </link>

    <joint name="${parent}_barrier_${name}_back_joint" type="fixed">
      <origin xyz="${-board_x / 2 + board_z / 2 - board_y} 0 ${joint_origin_z + barrier_z/2 - board_z/2}"/>
      <parent link="${parent}_main"/>
      <child link="${parent}_barrier_${name}_back_link"/>
    </joint>
  </macro>



  <macro name="iai_vdrawer" params="name parent *origin">
    <joint name="${name}_main_joint" type="prismatic">
      <xacro:insert_block name="origin"/>
      <parent link="${parent}"/>
      <child link="${name}_main"/>
      <axis xyz="1. 0. 0.0"/>
      <limit effort="300" lower="0" upper="0.48" velocity="10"/>
    </joint>

    <link name="${name}_main">
      <sphere_inertia radius="0" mass="0"/>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="0.025 0.295 1.325"/>
        </geometry>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="0.025 0.3 1.33"/>
        </geometry>
      </collision>
    </link>

    <joint name="${name}_handle_joint" type="fixed">
      <origin rpy="${-0.5*pi} 0 0" xyz="0.08 0.0 0.0"/>
      <parent link="${name}_main" />
      <child link="${name}_handle" />
    </joint>

    <link name="${name}_handle">
      <sphere_inertia radius="0" mass="0"/>
      <visual>
        <origin xyz="0 0 0" rpy="${0.5*pi} 0 0" />
        <geometry>
          <mesh filename="package://iai_kitchen/meshes/handles/VHandle130.obj"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="${0.5*pi} 0 0" />
        <geometry>
          <mesh filename="package://iai_kitchen/meshes/handles/VHandle130.obj"/>
        </geometry>
      </collision>
    </link>

    <iai_vdrawer_board name="0" parent="${name}" joint_origin_z="-0.6"/>
    <iai_vdrawer_board name="1" parent="${name}" joint_origin_z="-0.28"/>
    <iai_vdrawer_board name="2" parent="${name}" joint_origin_z="0.05"/>
    <iai_vdrawer_board name="3" parent="${name}" joint_origin_z="0.38"/>
  </macro>


  <macro name="iai_drawer_29" params="name parent width *origin">   
    
    <iai_generic_drawer name="${name}" parent="${parent}" width="${width}" height="29">
      <xacro:insert_block name="origin" />
      <origin xyz="0.3525 0 0.11" rpy="0 0 0" />
    </iai_generic_drawer>
  </macro>
  
  <macro name="iai_drawer_14" params="name parent width *origin">   
    
    <iai_generic_drawer name="${name}" parent="${parent}" width="${width}" height="14">
      <xacro:insert_block name="origin" />
      <origin xyz="0.3525 0 0.0175" rpy="0 0 0" />
    </iai_generic_drawer>
  </macro>
  
  
  
  <macro name="iai_generic_drawer" params="name parent width height *origin *handle_origin">   
    <link name="${name}_main">      
      <sphere_inertia radius="0" mass="0"/>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="package://iai_kitchen/meshes/drawers/Drawer_${width}_${height}.obj"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="package://iai_kitchen/meshes/drawers/Drawer_${width}_${height}.obj"/>
        </geometry>
      </collision>
    </link>

    <link name="${name}_handle">      
      <sphere_inertia radius="0" mass="0"/>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="package://iai_kitchen/meshes/handles/Handle${width}.obj"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="package://iai_kitchen/meshes/handles/Handle${width}.obj"/>
        </geometry>
      </collision>
    </link>

    <joint name="${name}_main_joint" type="prismatic">
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_main"/>
      <axis xyz="1. 0. 0.0"/>
      <limit effort="300" lower="0" upper="0.48" velocity="10"/>   
      <!--
          <limit effort="300" lower="0" upper="0" velocity="10"/>
      -->
    </joint>
    
    
    <joint name="${name}_handle_joint" type="fixed">
      <xacro:insert_block name="handle_origin"/>
      <parent link="${name}_main"/>
      <child link="${name}_handle"/>
      <axis xyz="0 0 1"/>
      <limit lower="0" upper="${0.5*pi}" effort="300" velocity="10"/>
    </joint>      
  </macro>
  
  
  <macro name="iai_generic_panel" params="name parent width *origin">   
    <link name="${name}">      
      <sphere_inertia radius="0" mass="0"/>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="package://iai_kitchen/meshes/drawers/Panel_${width}.obj"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="package://iai_kitchen/meshes/drawers/Panel_${width}.obj"/>
        </geometry>
      </collision>
    </link>
    
    <joint name="${name}_joint" type="fixed">
      <xacro:insert_block name="origin"/>
      <parent link="${parent}"/>
      <child link="${name}"/>
    </joint>      
  </macro>

  
  <!--- FIXME THIS should NOT be here -->
  <!--
      <link name="map">
      
      <sphere_inertia radius="0" mass="0"/>
      </link>
      <iai_drawer_60_14 name="drawer_1" parent="map">
      <origin rpy="0 0 0" xyz="0 0 1"/>
      </iai_drawer_60_14>
      
      <iai_drawer_80_14 name="drawer_2" parent="map">
      <origin rpy="0 0 0" xyz="0 0 2"/>
      </iai_drawer_80_14>
      
      <iai_drawer_60_29 name="drawer_3" parent="map">
      <origin rpy="0 0 0" xyz="0 0 3"/>
      </iai_drawer_60_29>
      
      <iai_drawer_80_29 name="drawer_4" parent="map">
      <origin rpy="0 0 0" xyz="0 0 4"/>
      </iai_drawer_80_29>
      
      <iai_drawer_100_29 name="drawer_5" parent="map">
      <origin rpy="0 0 0" xyz="0 0 5"/>
      </iai_drawer_100_29>
  -->
</robot>
