<?xml version="1.0"?>

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

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

  <macro name="iai_dish_washer" params="name parent *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/misc/DishWasher.dae"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="package://iai_kitchen/meshes/misc/DishWasher.dae"/>
        </geometry>
      </collision>
    </link>
    
    <link name="${name}_door">
      
      <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/misc/DishWasherDoor.dae"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="${0.5*pi} 0 0" />
        <geometry>
          <mesh filename="package://iai_kitchen/meshes/misc/DishWasherDoor.dae"/>
        </geometry>
      </collision>
    </link>

    <link name="${name}_door_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/Handle60.dae"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="package://iai_kitchen/meshes/handles/Handle60.dae"/>
        </geometry>
      </collision>
    </link>
    <!--
    <xacro:if value="simulated"/>
    -->
    <joint name="${name}_main_joint" type="revolute">
      <limit lower="0" upper="0" effort="300" velocity="10"/>
      
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_main"/>
    </joint>
    
    <xacro:if value="${simulated}">
    <joint name="${name}_door_joint" type="revolute">
      <origin xyz="0.27 0. -0.325" rpy="${-0.5*pi} 0 0" />
      <parent link="${name}_main"/>
      <child link="${name}_door"/>
      <axis xyz="0 0 1"/>
      <!-- FIXME make closed position stable instead of forbidding opening... -->
      <limit lower="0" upper="0" effort="300" velocity="10"/>
    </joint>
    
    </xacro:if>
    <xacro:unless value="${simulated}">
    <joint name="${name}_door_joint" type="revolute">
      <origin xyz="0.27 0. -0.325" rpy="${-0.5*pi} 0 0" />
      <parent link="${name}_main"/>
      <child link="${name}_door"/>
      <axis xyz="0 0 1"/>
      <!--
      <limit lower="0" upper="${0.5*pi}" effort="300" velocity="10"/>
      -->
      <limit lower="0" upper="${0.5*pi}" effort="300" velocity="10"/>
    </joint>
    
    </xacro:unless>
    <joint name="${name}_door_handle_joint" type="fixed">
      <origin xyz="0.0625 -0.6325 0." rpy="${-0.5*pi} 0 0" />
      <parent link="${name}_door"/>
      <child link="${name}_door_handle"/>
    </joint>
  </macro>

</robot>