<?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 robot link (without prefix)              -->
  <!-- inertial: Dictionary of inertia properties (see inertial.yaml) -->
  <!-- ============================================================== -->
  <xacro:macro name="franka-inertials" params="name inertials:=^">
      <xacro:unless value="${name in inertials}">${xacro.warning('No inertia properties defined for: ' + name)}</xacro:unless>
      <xacro:if value="${name in inertials}">
        <!-- Access inertia properties of link 'name' -->
        <xacro:property name="link_inertials" value="${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 robot link (without prefix)             -->
  <!-- rpy:   Rotation of the *_sc link relative to parent [rad]  -->
  <!-- self_collision_geometries:  self <collision> models        -->
  <!-- ========================================================== -->
  <xacro:macro name="link_with_sc" params="name no_prefix:='false' rpy:='0 0 0' **self_collision_geometries gazebo:=false with_sc:=false">
    <xacro:property name="prefix" value="${'' if no_prefix else arm_prefix + arm_id + '_'}" />
    <!-- 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_arms/${arm_id}/visual/${name}.dae" />
        </geometry>
      </visual>
      <collision name="${prefix}${name}_collision">
        <geometry>
          <mesh filename="package://franka_description/meshes/robot_arms/${arm_id}/collision/${name}.stl" />
        </geometry>
      </collision>
      <xacro:franka-inertials name="${name}" />
    </link>

    <!-- sub-link defining coarse geometries of real robot'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>

  <!-- ========================================================== -->
  <!-- Adds the required tags to simulate one joint in gazebo     -->
  <!--                                                            -->
  <!-- joint - Name of the fer joint to simulate                  -->
  <!-- transmission - type of the transmission of the joint       -->
  <!-- ========================================================== -->
  <xacro:macro name="gazebo-joint" params="joint transmission:=hardware_interface/EffortJointInterface">
    <gazebo reference="${joint}">
      <!-- Needed for ODE to output external wrenches on joints -->
      <provideFeedback>true</provideFeedback>
    </gazebo>

    <transmission name="${joint}_transmission">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${joint}">
        <hardwareInterface>${transmission}</hardwareInterface>
      </joint>
      <actuator name="${joint}_motor">
        <hardwareInterface>${transmission}</hardwareInterface>
      </actuator>
    </transmission>
  </xacro:macro>

  <xacro:macro name="gazebo-friction" params="link mu">
    <gazebo reference="${link}">
      <collision>
        <max_contacts>10</max_contacts>
        <surface>
          <contact>
            <ode>
              <!-- These two parameter need application specific tuning. -->
              <!-- Usually you want no "snap out" velocity and a generous -->
              <!-- penetration depth to keep the grasping stable -->
              <max_vel>0</max_vel>
              <min_depth>0.001</min_depth>
            </ode>
          </contact>
          <friction>
            <ode>
              <mu>${mu}</mu>
              <mu2>${mu}</mu2>
            </ode>
          </friction>
          <bounce/>
        </surface>
      </collision>
    </gazebo>
  </xacro:macro>

  <!-- ========================================================== -->
  <!-- Adds the required tags to provide a `FrankaStateInterface` -->
  <!-- when simulating with franka_hw_sim plugin                  -->
  <!--                                                            -->
  <!-- arm_id - Arm ID of the fer to simulate (default 'fer')     -->
  <!-- ========================================================== -->
  <xacro:macro name="transmission-franka-state" params="arm_id:=fer">
    <transmission name="${arm_prefix}${arm_id}_franka_state">
      <type>franka_hw/FrankaStateInterface</type>
      <joint name="${arm_prefix}${arm_id}_joint1"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_prefix}${arm_id}_joint2"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_prefix}${arm_id}_joint3"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_prefix}${arm_id}_joint4"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_prefix}${arm_id}_joint5"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_prefix}${arm_id}_joint6"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_prefix}${arm_id}_joint7"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>

      <actuator name="${arm_prefix}${arm_id}_motor1"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_prefix}${arm_id}_motor2"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_prefix}${arm_id}_motor3"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_prefix}${arm_id}_motor4"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_prefix}${arm_id}_motor5"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_prefix}${arm_id}_motor6"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_prefix}${arm_id}_motor7"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
    </transmission>
  </xacro:macro>

  <!-- ============================================================ -->
  <!-- Adds the required tags to provide a `FrankaModelInterface`   -->
  <!-- when simulating with franka_hw_sim plugin                    -->
  <!--                                                              -->
  <!-- arm_id - Arm ID of the fer to simulate (default 'fer')       -->
  <!-- root   - Joint name of the base of the robot                 -->
  <!-- tip    - Joint name of the tip of the robot (flange or hand) -->
  <!-- ============================================================ -->
  <xacro:macro name="transmission-franka-model" params="arm_id:=fer root:=fer_joint1 tip:=fer_joint7">
    <transmission name="${arm_prefix}${arm_id}_franka_model">
      <type>franka_hw/FrankaModelInterface</type>
      <joint name="${root}">
        <role>root</role>
        <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
      </joint>
      <joint name="${tip}">
        <role>tip</role>
        <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
      </joint>

      <actuator name="${root}_motor_root"><hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface></actuator>
      <actuator name="${tip}_motor_tip"  ><hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface></actuator>
    </transmission>
  </xacro:macro>

  <xacro:macro name="inertia-cylinder" params="mass radius h">
    <inertial>
      <mass value="${mass}" />
      <inertia ixx="${1./12 * mass * (3 * radius**2 + h**2)}" ixy = "0" ixz = "0"
               iyy="${1./12 * mass * (3 * radius**2 + h**2)}" iyz = "0"
               izz="${1./2  * mass * radius**2}" />
    </inertial>
  </xacro:macro>

  <!-- ========================================================================= -->
  <!-- Adds the <limit ... /> & <safety_controller/> tags given a config file    -->
  <!-- of joint limits                                                           -->
  <!--                                                                           -->
  <!-- config - YAML struct defining joint limits (e.g. fer/joint_limits.yaml)   -->
  <!-- name   - Name of the joint for which to add limits to                     -->
  <!-- ========================================================================= -->
  <xacro:macro name="franka-limits" params="config name">
      <xacro:property name="limits" value="${config[name]['limit']}" lazy_eval="false" />
      <limit lower="${limits.lower}"
             upper="${limits.upper}"
             effort="${limits.effort}"
             velocity="${limits.velocity}" />
      <safety_controller k_position="100.0"
                         k_velocity="40.0"
                         soft_lower_limit="${limits.lower}"
                         soft_upper_limit="${limits.upper}" />
  </xacro:macro>

  <!-- ========================================================================= -->
  <!-- Adds the <dynamics ... /> tag given a config file of joint dynamics       -->
  <!--                                                                           -->
  <!-- config - YAML struct defining joint dynamics (e.g. fer/dynamics.yaml)     -->
  <!-- name   - Name of the joint for which to add dynamics to                   -->
  <!-- ========================================================================= -->
  <xacro:macro name="franka-dynamics" params="config name">
      <xacro:property name="dynamics" value="${config[name]['dynamic']}" lazy_eval="false" />
      <dynamics D="${dynamics.D}"
             K="${dynamics.K}"
             damping="${dynamics.damping}"
             friction="${dynamics.friction}"
             mu_coulomb="${dynamics.mu_coulomb}"
             mu_viscous="${dynamics.mu_viscous}" />
  </xacro:macro>

  <!-- ========================================================================= -->
  <!-- Adds the <kinematics ... /> tag given a config file of joint kinematics   -->
  <!--                                                                           -->
  <!-- config - YAML struct defining joint kinematics (e.g. fer/kinematics.yaml) -->
  <!-- name   - Name of the joint for which to add kinematics to                 -->
  <!-- ========================================================================= -->
  <xacro:macro name="franka-kinematics" params="config name">
      <xacro:property name="kinematics" value="${config[name]['kinematic']}" lazy_eval="false" />
      <origin rpy="${kinematics.roll} ${kinematics.pitch} ${kinematics.yaw}"
             xyz="${kinematics.x} ${kinematics.y} ${kinematics.z}" />
  </xacro:macro>

</robot>
