<?xml version='1.0' encoding='utf-8'?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fer">

  <xacro:include filename="$(find moonshot_franka_description)/robots/common/utils.xacro" />

  <!-- safety_distance: Minimum safety distance in [m] by which the collision volumes are expanded and which is enforced during robot motions -->
  <!-- arm_id: Namespace of the robot arm. Serves to differentiate between arms in case of multiple instances. -->
  <!-- joint_limits: description of the joint limits that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find moonshot_franka_description)/robots/fr3/joint_limits.yaml')} -->
  <!-- kinematics: description of the kinematics that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find moonshot_franka_description)/robots/fr3/kinematics.yaml')} -->
  <!-- inertials: description of the inertials that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find moonshot_franka_description)/robots/fr3/inertials.yaml')} -->
  <!-- dynamics: description of the dynamics that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find moonshot_franka_description)/robots/fr3/dynamics.yaml')} -->

  <xacro:macro name="franka_arm" params="arm_id arm_prefix:='' no_prefix:=false description_pkg:='moonshot_franka_description' connected_to:='base' xyz:='0 0 0' rpy:='0 0 0' gazebo:='false' safety_distance:=0 joint_limits inertials kinematics dynamics with_sc:=false" >

    <!-- Define a property that defaults to 'arm_prefix_arm_id' concatenated with an underscore if 'no_prefix' is not set -->
    <xacro:property name="prefix" value="${'' if no_prefix else arm_prefix + arm_id + '_'}" />

    <xacro:if value="${gazebo}">
      <xacro:property name="connected_to" value="" />
    </xacro:if>

    <xacro:unless value="${not connected_to}">
      <link name="${connected_to}">
      </link>
      <joint name="${prefix}${connected_to}_joint" type="fixed">
        <parent link="${connected_to}"/>
        <child link="${prefix}link0"/>
        <origin rpy="${rpy}" xyz="${xyz}"/>
      </joint>
    </xacro:unless>

    <xacro:link_with_sc no_prefix="${no_prefix}" name="link0" gazebo="${gazebo}" with_sc="${with_sc}">
      <self_collision_geometries>
        <xacro:collision_capsule xyz="-0.075 0 0.06" direction="x" radius="${0.06+safety_distance}" length="0.03" />
      </self_collision_geometries>
    </xacro:link_with_sc>

    <xacro:link_with_sc no_prefix="${no_prefix}" name="link1" gazebo="${gazebo}" with_sc="${with_sc}">
      <self_collision_geometries>
        <xacro:collision_capsule xyz="0 0 -191.5e-3" radius="${0.06+safety_distance}" length="0.283" />
      </self_collision_geometries>
    </xacro:link_with_sc>

    <joint name="${prefix}joint1" type="revolute">
      <xacro:franka-kinematics name="joint1" config="${kinematics}" />
      <parent link="${prefix}link0" />
      <child link="${prefix}link1" />
      <axis xyz="0 0 1" />
      <xacro:franka-limits name="joint1" config="${joint_limits}" />
      <xacro:franka-dynamics name="joint1" config="${dynamics}" />
    </joint>

    <xacro:link_with_sc no_prefix="${no_prefix}" name="link2" gazebo="${gazebo}" with_sc="${with_sc}">
      <self_collision_geometries>
        <xacro:collision_capsule xyz="0 0 0" radius="${0.06+safety_distance}" length="0.12" />
      </self_collision_geometries>
    </xacro:link_with_sc>

    <joint name="${prefix}joint2" type="revolute">
      <xacro:franka-kinematics name="joint2" config="${kinematics}" />
      <parent link="${prefix}link1" />
      <child link="${prefix}link2" />
      <axis xyz="0 0 1" />
      <xacro:franka-limits name="joint2" config="${joint_limits}" />
      <xacro:franka-dynamics name="joint2" config="${dynamics}" />
    </joint>

    <xacro:link_with_sc no_prefix="${no_prefix}" name="link3" gazebo="${gazebo}" with_sc="${with_sc}">
      <self_collision_geometries>
        <xacro:collision_capsule xyz="0 0 -0.145" radius="${0.06+safety_distance}" length="0.15" />
      </self_collision_geometries>
    </xacro:link_with_sc>

    <joint name="${prefix}joint3" type="revolute">
      <xacro:franka-kinematics name="joint3" config="${kinematics}" />
      <parent link="${prefix}link2" />
      <child link="${prefix}link3" />
      <axis xyz="0 0 1" />
      <xacro:franka-limits name="joint3" config="${joint_limits}" />
      <xacro:franka-dynamics name="joint3" config="${dynamics}" />
    </joint>

    <xacro:link_with_sc no_prefix="${no_prefix}" name="link4" gazebo="${gazebo}" with_sc="${with_sc}">
      <self_collision_geometries>
        <xacro:collision_capsule xyz="0 0 0" radius="${0.06+safety_distance}" length="0.12" />
     </self_collision_geometries>
    </xacro:link_with_sc>

    <joint name="${prefix}joint4" type="revolute">
      <xacro:franka-kinematics name="joint4" config="${kinematics}" />
      <parent link="${prefix}link3" />
      <child link="${prefix}link4" />
      <axis xyz="0 0 1" />
      <xacro:franka-limits name="joint4" config="${joint_limits}" />
      <xacro:franka-dynamics name="joint4" config="${dynamics}" />
    </joint>

    <xacro:link_with_sc no_prefix="${no_prefix}" name="link5" gazebo="${gazebo}" with_sc="${with_sc}">
      <self_collision_geometries>
        <xacro:collision_capsule xyz="0 0 -0.26"    radius="${0.060+safety_distance}" length="0.10" />
        <xacro:collision_capsule xyz="0 0.08 -0.13" radius="${0.025+safety_distance}" length="0.14" />
      </self_collision_geometries>
    </xacro:link_with_sc>

    <joint name="${prefix}joint5" type="revolute">
      <xacro:franka-kinematics name="joint5" config="${kinematics}" />
      <parent link="${prefix}link4" />
      <child link="${prefix}link5" />
      <axis xyz="0 0 1" />
      <xacro:franka-limits name="joint5" config="${joint_limits}" />
      <xacro:franka-dynamics name="joint5" config="${dynamics}" />
    </joint>

    <xacro:link_with_sc no_prefix="${no_prefix}" name="link6" gazebo="${gazebo}" with_sc="${with_sc}">
      <self_collision_geometries>
        <xacro:collision_capsule xyz="0 0 -0.03" radius="${0.05+safety_distance}" length="0.08" />
      </self_collision_geometries>
    </xacro:link_with_sc>

    <joint name="${prefix}joint6" type="revolute">
      <xacro:franka-kinematics name="joint6" config="${kinematics}" />
      <parent link="${prefix}link5" />
      <child link="${prefix}link6" />
      <axis xyz="0 0 1" />
      <xacro:franka-limits name="joint6" config="${joint_limits}" />
      <xacro:franka-dynamics name="joint6" config="${dynamics}" />
    </joint>

    <xacro:link_with_sc no_prefix="${no_prefix}" name="link7" gazebo="${gazebo}" rpy="0 0 ${pi/4}" with_sc="${with_sc}">
      <self_collision_geometries>
        <xacro:collision_capsule xyz="0 0 0.01" direction="z" radius="${0.04+safety_distance}" length="0.14" />
        <xacro:collision_capsule xyz="0.06 0 0.082" direction="x" radius="${0.03+safety_distance}"  length="0.01" />
      </self_collision_geometries>
    </xacro:link_with_sc>

    <joint name="${prefix}joint7" type="revolute">
      <xacro:franka-kinematics name="joint7" config="${kinematics}" />
      <parent link="${prefix}link6"/>
      <child link="${prefix}link7"/>
      <axis xyz="0 0 1"/>
      <xacro:franka-limits name="joint7" config="${joint_limits}" />
      <xacro:franka-dynamics name="joint7" config="${dynamics}" />
    </joint>

    <link name="${prefix}link8"/>

    <joint name="${prefix}joint8" type="fixed">
      <xacro:franka-kinematics name="joint8" config="${kinematics}" />
      <parent link="${prefix}link7" />
      <child link="${prefix}link8" />
    </joint>
  </xacro:macro>
</robot>
