<?xml version="1.0"?>
<!--

  Copyright (c) 2021 PAL Robotics S.L. All rights reserved.
  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at
      http://www.apache.org/licenses/LICENSE-2.0
  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
-->

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

  <!--File includes-->
  <xacro:include filename="$(find tiago_description)/urdf/deg_to_rad.xacro" />
  <xacro:include filename="$(find hey5_description)/urdf/hey5_hand.urdf.xacro" />

  <!--
    Utiliy to create a tiago end effector (right or left).

    Parameters:
    - name: End effector name. The created links and joints are prepended with 'name_'
    - parent: Parent link
    - reflect: Set to 1 for a right end effector, or to -1 for a left end effector
  -->
  <xacro:macro name="tiago_end_effector" params="name parent type reflect is_dual:=false">

  <!-- PAL Gripper -->
  <xacro:if value="${type == 'pal-gripper'}">
    <xacro:include filename="$(find pal_gripper_description)/urdf/gripper.urdf.xacro" />
    <xacro:pal_gripper parent="${parent}" name="${name}">
      <origin xyz="0.01 0 0" rpy="${90*deg_to_rad} ${90*deg_to_rad} ${-90*deg_to_rad}"/>
    </xacro:pal_gripper>
  </xacro:if>

  <!-- Schunk WSG -->
  <xacro:if value="${type == 'schunk-wsg'}">
    <xacro:include filename="$(find pal_wsg_gripper_description)/urdf/gripper.urdf.xacro" />
    <xacro:include filename="$(find pal_wsg_gripper_description)/urdf/fingertip_sensor.urdf.xacro" />

    <xacro:pal_wsg_gripper parent="${parent}" name="${name}">
      <origin xyz="0.01 0 0" rpy="${90*deg_to_rad} ${90*deg_to_rad} ${-90*deg_to_rad}"/>
    </xacro:pal_wsg_gripper>
    <!-- Fingertip sensors -->
    <xacro:fingertip_sensor name="${name}_left" parent="${name}_left_finger">
      <origin xyz="0.035 0 -0.1465" rpy="0 ${-90*deg_to_rad} 0"/>
    </xacro:fingertip_sensor>
    <xacro:fingertip_sensor name="${name}_right" parent="${name}_right_finger">
      <origin xyz="-0.035 0 -0.1465" rpy="0 ${90*deg_to_rad} 0"/>
    </xacro:fingertip_sensor>
  </xacro:if>

  <!-- Hand -->
  <xacro:if value="${type == 'pal-hey5'}">
    <xacro:hey5_hand parent="${parent}" name="${name}" reflect="${reflect}">
      <origin xyz="0 0 0" rpy="0 0 0"/>
    </xacro:hey5_hand>

    <joint name="${name}_safety_box_joint" type="fixed">
      <parent link="${parent}"/>
      <child link="${name}_safety_box"/>
      <origin xyz="0.1 0.02 ${reflect * 0.02}" rpy="0 0 0"/>
    </joint>

    <link name="${name}_safety_box">
      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <mass value="0.0001" />
        <inertia ixx="0.0001"  ixy="0"  ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
      </inertial>
      <!--
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.2 0.115 0.18" />
        </geometry>
      </visual>
      -->
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.2 0.115 0.18" />
        </geometry>
      </collision>
    </link>
  </xacro:if>

  <!-- Robotiq 2F-85 Gripper -->
  <xacro:if value="${type == 'robotiq-2f-85'}">
    <xacro:include filename="$(find pal_robotiq_85_description)/urdf/robotiq_85_gripper.urdf.xacro" />
    <xacro:robotiq_85_gripper prefix="${name}_" parent="${parent}" >
      <origin xyz="0.01 0 0" rpy="${90*deg_to_rad} ${0*deg_to_rad} ${90*deg_to_rad}"/>
    </xacro:robotiq_85_gripper>
  </xacro:if>

  <!-- Robotiq 2F-140 Gripper -->
  <xacro:if value="${type == 'robotiq-2f-140'}">
    <xacro:include filename="$(find pal_robotiq_140_description)/urdf/robotiq_140_gripper.urdf.xacro" />
    <xacro:robotiq_140_gripper prefix="${name}_" parent="${parent}" >
      <origin xyz="0.01 0 0" rpy="${90*deg_to_rad} ${0*deg_to_rad} ${90*deg_to_rad}"/>
    </xacro:robotiq_140_gripper>
  </xacro:if>

  <!-- Robotiq Epick Gripper -->
  <xacro:if value="${type == 'robotiq-epick'}">
    <xacro:include filename="$(find pal_robotiq_epick_description)/urdf/robotiq_epick_model_macro.xacro" />
    <xacro:epick_hand  prefix="${name}_" parent="${parent}">
      <origin xyz="0.01 0 0" rpy="${90*deg_to_rad} ${0*deg_to_rad} ${90*deg_to_rad}"/>
    </xacro:epick_hand>
  </xacro:if>

  <!-- Custom -->
  <xacro:if value="${type == 'custom' and not is_dual}">
    <xacro:include filename="$(find custom_ee_description)/urdf/end-effector.urdf.xacro" />
    <xacro:end_effector parent="${parent}">
      <origin xyz="0.01 0 0" rpy="${90*deg_to_rad} ${90*deg_to_rad} ${-90*deg_to_rad}"/>
    </xacro:end_effector>
  </xacro:if>

  <!-- Custom dual-->
  <xacro:if value="${type == 'custom' and is_dual}">
    <xacro:if value="${reflect == 1}"> <!-- If right -->
    <xacro:include filename="$(find custom_dual_ee_description)/urdf/end-effector-right.urdf.xacro" />
    <xacro:end_effector_right parent="${parent}">
      <origin xyz="0.01 0 0" rpy="${90*deg_to_rad} ${90*deg_to_rad} ${-90*deg_to_rad}"/>
    </xacro:end_effector_right>
    </xacro:if>
    <xacro:if value="${reflect == -1}"> <!-- If left -->
    <xacro:include filename="$(find custom_dual_ee_description)/urdf/end-effector-left.urdf.xacro" />
    <xacro:end_effector_left parent="${parent}">
      <origin xyz="0.01 0 0" rpy="${90*deg_to_rad} ${90*deg_to_rad} ${-90*deg_to_rad}"/>
    </xacro:end_effector_left>
    </xacro:if>
  </xacro:if>

  </xacro:macro>

</robot>
