<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">
  <!-- safety_distance: Minimum safety distance in [m] by which the collision volumes are expanded and which is enforced during robot motions -->
  <xacro:macro name="franka_hand" params="connected_to:='' arm_id arm_prefix:='' ee_id ee_inertials rpy_ee:='0 0 0' xyz_ee:='0 0 0' tcp_xyz:='0 0 0.1034' tcp_rpy:='0 0 0' safety_distance:=0 gazebo:=false description_pkg:=moonshot_franka_description with_sc:=false">
    <xacro:property name="ee_prefix" default="robot_"/>
    <xacro:unless value="${arm_id == ''}">
      <xacro:property name="ee_prefix" value="${arm_prefix}${arm_id}_" />
    </xacro:unless>
    <xacro:unless value="${connected_to == ''}">
      <joint name="${ee_prefix}hand_joint" type="fixed">
        <parent link="${connected_to}" />
        <child link="${ee_prefix}hand" />
        <origin xyz="${xyz_ee}" rpy="${rpy_ee}" />
      </joint>
    </xacro:unless>

    <xacro:ee_link_with_sc name="hand" gazebo="${gazebo}" with_sc="${with_sc}">
      <self_collision_geometries>
        <xacro:collision_capsule xyz="0 0 0.04" direction="y" radius="${0.04+safety_distance}" length="0.1" />
        <xacro:collision_capsule xyz="0 0 0.10" direction="y" radius="${0.02+safety_distance}" length="0.1" />
      </self_collision_geometries>
    </xacro:ee_link_with_sc>

    <!-- Define the hand_tcp frame -->
    <link name="${ee_prefix}hand_tcp" />
    <joint name="${ee_prefix}hand_tcp_joint" type="fixed">
      <origin xyz="${tcp_xyz}" rpy="${tcp_rpy}" />
      <parent link="${ee_prefix}hand" />
      <child link="${ee_prefix}hand_tcp" />
    </joint>
    <link name="${ee_prefix}leftfinger">
      <visual name="${ee_prefix}leftfinger_visual">
        <geometry>
          <mesh filename="package://${description_pkg}/meshes/robot_ee/${ee_id}/visual/finger.dae" />
        </geometry>
      </visual>
      <!-- screw mount -->
      <collision>
        <origin xyz="0 18.5e-3 11e-3" rpy="0 0 0" />
        <geometry>
          <box size="22e-3 15e-3 20e-3" />
        </geometry>
      </collision>
      <!-- cartriage sledge -->
      <collision>
        <origin xyz="0 6.8e-3 2.2e-3" rpy="0 0 0" />
        <geometry>
          <box size="22e-3 8.8e-3 3.8e-3" />
        </geometry>
      </collision>
      <!-- diagonal finger -->
      <collision>
        <origin xyz="0 15.9e-3 28.35e-3" rpy="${pi/6} 0 0" />
        <geometry>
          <box size="17.5e-3 7e-3 23.5e-3" />
        </geometry>
      </collision>
      <!-- rubber tip with which to grasp -->
      <collision>
        <origin xyz="0 7.58e-3 45.25e-3" rpy="0 0 0" />
        <geometry>
          <box size="17.5e-3 15.2e-3 18.5e-3" />
        </geometry>
      </collision>
      <xacro:if value="${gazebo}">
        <xacro:ee-inertials name="leftfinger" />
      </xacro:if>
    </link>
    <link name="${ee_prefix}rightfinger">
      <visual name="${ee_prefix}rightfinger_visual">
        <origin xyz="0 0 0" rpy="0 0 ${pi}" />
        <geometry>
          <mesh filename="package://${description_pkg}/meshes/robot_ee/${ee_id}/visual/finger.dae" />
        </geometry>
      </visual>
      <!-- screw mount -->
      <collision>
        <origin xyz="0 -18.5e-3 11e-3" rpy="0 0 0" />
        <geometry>
          <box size="22e-3 15e-3 20e-3" />
        </geometry>
      </collision>
      <!-- cartriage sledge -->
      <collision>
        <origin xyz="0 -6.8e-3 2.2e-3" rpy="0 0 0" />
        <geometry>
          <box size="22e-3 8.8e-3 3.8e-3" />
        </geometry>
      </collision>
      <!-- diagonal finger -->
      <collision>
        <origin xyz="0 -15.9e-3 28.35e-3" rpy="${-pi/6} 0 0" />
        <geometry>
          <box size="17.5e-3 7e-3 23.5e-3" />
        </geometry>
      </collision>
      <!-- rubber tip with which to grasp -->
      <collision>
        <origin xyz="0 -7.58e-3 45.25e-3" rpy="0 0 0" />
        <geometry>
          <box size="17.5e-3 15.2e-3 18.5e-3" />
        </geometry>
      </collision>
      <xacro:if value="${gazebo}">
        <xacro:ee-inertials name="rightfinger" />
      </xacro:if>
    </link>
    <joint name="${ee_prefix}finger_joint1" type="prismatic">
      <parent link="${ee_prefix}hand" />
      <child link="${ee_prefix}leftfinger" />
      <origin xyz="0 0 0.0584" rpy="0 0 0" />
      <axis xyz="0 1 0" />
      <limit effort="100" lower="0.0" upper="0.04" velocity="0.2" />
      <dynamics damping="0.3" />
    </joint>
    <joint name="${ee_prefix}finger_joint2" type="prismatic">
      <parent link="${ee_prefix}hand" />
      <child link="${ee_prefix}rightfinger" />
      <origin xyz="0 0 0.0584" rpy="0 0 0" />
      <axis xyz="0 -1 0" />
      <limit effort="100" lower="0.0" upper="0.04" velocity="0.2" />
      <mimic joint="${ee_prefix}finger_joint1" />
      <dynamics damping="0.3" />
    </joint>
  </xacro:macro>
</robot>
