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

  <!-- ============================================================== -->
  <!-- Macro to add an <inertial> tag based on yaml-load properties   -->
  <!--                                                                -->
  <!-- name:     Name of the end_effector (without prefix)              -->
  <!-- ee_inertials: Dictionary of inertia properties (see inertial.yaml) -->
  <!-- ============================================================== -->
  <xacro:macro name="ee-inertials" params="name ee_inertials:=^">
      <xacro:unless value="${name in ee_inertials}">${xacro.warning('No inertia properties defined for: ' + name)}</xacro:unless>
      <xacro:if value="${name in ee_inertials}">
        <!-- Access inertia properties of link 'name' -->
        <xacro:property name="link_inertials" value="${ee_inertials[name]}" lazy_eval="false" />
        <inertial>
          <origin rpy="${link_inertials.origin.rpy}" xyz="${link_inertials.origin.xyz}" />
          <mass value="${link_inertials.mass}" />
          <xacro:if value="${'inertia' in link_inertials}">
            <xacro:property name="I" value="${link_inertials.inertia}" />
            <inertia ixx="${I.xx}" ixy="${I.xy}" ixz="${I.xz}" iyy="${I.yy}" iyz="${I.yz}" izz="${I.zz}" />
          </xacro:if>
        </inertial>
      </xacro:if>
  </xacro:macro>

  <!-- ========================================================== -->
  <!-- Macro to add a single link, both with                      -->
  <!--  * detailed meshes for environmental collision checking    -->
  <!--  * and coarse geometry models for self-collision checking  -->
  <!--    (only if 'gazebo' param is set)                         -->
  <!--                                                            -->
  <!-- name:  Name of the end-effector (without prefix)             -->
  <!-- rpy:   Rotation of the *_sc link relative to parent [rad]  -->
  <!-- self_collision_geometries:  self <collision> models        -->
  <!-- ========================================================== -->
  <xacro:macro name="ee_link_with_sc" params="name prefix=${ee_prefix} rpy:='0 0 0' **self_collision_geometries gazebo:=false with_sc:=false">
    <!-- sub-link defining detailed meshes for collision with environment -->
    <link name="${prefix}${name}">
      <visual name="${prefix}${name}_visual">
        <geometry>
          <mesh filename="package://${description_pkg}/meshes/robot_ee/${ee_id}/visual/${name}.dae" />
        </geometry>
      </visual>
      <collision name="${prefix}${name}_collision">
        <geometry>
          <mesh filename="package://franka_description/meshes/robot_ee/${ee_id}/collision/${name}.stl" />
        </geometry>
      </collision>
      <xacro:ee-inertials name="${name}" />
    </link>

    <!-- sub-link defining coarse geometries of real end-effector's internal self-collision -->
    <xacro:if value="${with_sc}">
      <link name="${prefix}${name}_sc">
        <xacro:unless value="${gazebo}">
          <xacro:insert_block name="self_collision_geometries" />
        </xacro:unless>
      </link>
      <!-- fixed joint between both sub-links -->
      <joint name="${prefix}${name}_sc_joint" type="fixed">
        <origin rpy="${rpy}" />
        <parent link="${prefix}${name}" />
        <child link="${prefix}${name}_sc" />
      </joint>
    </xacro:if>
  </xacro:macro>

    <!-- =========================================================== -->
  <!-- Add a <collision> tag with a capsule, made from a cylinder  -->
  <!-- with two spheres at its caps. The capsule will always be    -->
  <!-- aligned with the axis of 'direction' you pass along.        -->
  <!--                                                             -->
  <!-- radius: Radii of both the cylinder and both spheres [m]     -->
  <!-- length: Length of the cylinder/distance between the centers -->
  <!--         of the spheres. NOT overall length of capsule!      -->
  <!-- xyz:    Position of the center of the capsule/cylinder      -->
  <!-- direction: One of { x, y, z, -x, -y, -z }                   -->
  <!-- =========================================================== -->
  <xacro:macro name="collision_capsule" params="radius length xyz:='0 0 0' direction:='z'">
    <xacro:property name="r" value="${pi/2.0 if 'y' in direction else 0}" />
    <xacro:property name="p" value="${pi/2.0 if 'x' in direction else 0}" />
    <xacro:property name="y" value="0" />
    <xacro:property name="x" value="${xyz.split(' ')[0]}" />
    <xacro:property name="y" value="${xyz.split(' ')[1]}" />
    <xacro:property name="z" value="${xyz.split(' ')[2]}" />
    <!-- Sphere center offsets from center of cylinder -->
    <xacro:property name="sx" value="${length / 2.0 if 'x' in direction else 0}" />
    <xacro:property name="sy" value="${length / 2.0 if 'y' in direction else 0}" />
    <xacro:property name="sz" value="${length / 2.0 if 'z' in direction else 0}" />

    <collision>
      <origin xyz="${x} ${y} ${z}" rpy="${r} ${p} ${y}"/>
      <geometry>
        <cylinder radius="${radius}" length="${length}" />
      </geometry>
    </collision>
    <collision>
      <origin xyz="${x+sx} ${y+sy} ${z+sz}" />
      <geometry>
        <sphere radius="${radius}" />
      </geometry>
    </collision>
    <collision>
      <origin xyz="${x-sx} ${y-sy} ${z-sz}" />
      <geometry>
        <sphere radius="${radius}" />
      </geometry>
    </collision>
  </xacro:macro>

</robot>