<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!-- This file is tmw branch copy of iiwa4.xacro but contains few modifications listed in import_recipe.txt -->
  <!-- Import all Gazebo-customization elements, including Gazebo colors -->
  <xacro:include filename="iiwa.gazebo.xacro" />
  <!-- Import Transmissions -->
  <xacro:include filename="iiwa.transmission.xacro" />
  <!-- Include Utilities -->
  <xacro:include filename="utilities.xacro" />
  
  <!-- some constants -->
  <xacro:property name="safety_controller_k_pos" value="100" />
  <xacro:property name="safety_controller_k_vel" value="2" /> 
  <xacro:property name="joint_damping" value="0.5" />
  <xacro:property name="max_effort" value="300"/>
  <xacro:property name="max_velocity" value="10"/>
  
  <xacro:macro name="iiwa14" params="parent hardware_interface robot_name *origin">
    
    <!--joint between {parent} and link_0-->
    <joint name="${parent}_${robot_name}_joint" type="fixed">
      <xacro:insert_block name="origin"/>
      <parent link="${parent}"/>
      <child link="${robot_name}_link_0"/>
    </joint>
    
    <link name="${robot_name}_link_0">
      <inertial>
        <origin xyz="-0.1 0 0.07" rpy="0 0 0"/>
        <mass value="5"/>
        <inertia ixx="0.05"  ixy="0"  ixz="0" iyy="0.06" iyz="0" izz="0.03" />
      </inertial>
      
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/visual/link_0.stl"/>
        </geometry>
        <material name="Grey"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/collision/link_0.stl"/>
        </geometry>
        <material name="Grey"/>
      </collision>
      
      <self_collision_checking>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <capsule radius="0.15" length="0.25"/>
        </geometry>
      </self_collision_checking>
      
    </link>
    
    <!-- joint between link_0 and link_1 -->
    <joint name="${robot_name}_joint_1" type="revolute">
      <parent link="${robot_name}_link_0"/>
      <child link="${robot_name}_link_1"/>
      <origin xyz="0 0 0.1575" rpy="0 0 0"/>
      <axis xyz="0 0 1"/>
      <limit lower="${-170 * PI / 180}" upper="${170 * PI / 180}"
             effort="${max_effort}" velocity="${max_velocity}" />
      <safety_controller soft_lower_limit="${-168 * PI / 180}"
                         soft_upper_limit="${168 * PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/> 
      <dynamics damping="${joint_damping}"/>
    </joint>
    
    <link name="${robot_name}_link_1">
      <inertial>
        <origin xyz="0 -0.03 0.12" rpy="0 0 0"/>
        <mass value="4"/>
        <inertia ixx="0.1"  ixy="0"  ixz="0" iyy="0.09" iyz="0" izz="0.02" />
      </inertial>
      
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/visual/link_1.stl"/>
        </geometry>
        <material name="Orange"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/collision/link_1.stl"/>
        </geometry>
        <material name="Orange"/>
      </collision>
    </link>
    
    <!-- joint between link_1 and link_2 -->
    <joint name="${robot_name}_joint_2" type="revolute">
      <parent link="${robot_name}_link_1"/>
      <child link="${robot_name}_link_2"/>
      <origin xyz="0 0 0.2025" rpy="${PI / 2}   0 ${PI}"/>
      <axis xyz="0 0 1"/>
      <limit lower="${-120 * PI / 180}" upper="${120 * PI / 180}"
             effort="${max_effort}" velocity="${max_velocity}" />
      <safety_controller soft_lower_limit="${-118 * PI / 180}"
                         soft_upper_limit="${118 * PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/> 
      <dynamics damping="${joint_damping}"/>
    </joint>
    
    <link name="${robot_name}_link_2">
      <inertial>
        <origin xyz="0.0003 0.059 0.042" rpy="0 0 0"/>
        <mass value="4"/>
        <inertia ixx="0.05"  ixy="0"  ixz="0" iyy="0.018" iyz="0" izz="0.044" />
      </inertial>
      
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/visual/link_2.stl"/>
        </geometry>
        <material name="Orange"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/collision/link_2.stl"/>
        </geometry>
        <material name="Orange"/>
      </collision>
    </link>
    
    <!-- joint between link_2 and link_3 -->
    <joint name="${robot_name}_joint_3" type="revolute">
      <parent link="${robot_name}_link_2"/>
      <child link="${robot_name}_link_3"/>
      <origin xyz="0 0.2045 0" rpy="${PI / 2} 0 ${PI}"/>
      <axis xyz="0 0 1"/>
      <limit lower="${-170 * PI / 180}" upper="${170 * PI / 180}"
             effort="${max_effort}" velocity="${max_velocity}" />
      <safety_controller soft_lower_limit="${-168 * PI / 180}"
                         soft_upper_limit="${168 * PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/> 
      <dynamics damping="${joint_damping}"/>
    </joint>
    
    <link name="${robot_name}_link_3">
      <inertial>
        <origin xyz="0 0.03 0.13" rpy="0 0 0"/>
        <mass value="3"/>
        <inertia ixx="0.08"  ixy="0"  ixz="0" iyy="0.075" iyz="0" izz="0.01" />
      </inertial>
      
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/visual/link_3.stl"/>
        </geometry>
        <material name="Orange"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/collision/link_3.stl"/>
        </geometry>
        <material name="Orange"/>
      </collision>
    </link>
    
    <!-- joint between link_3 and link_4 -->
    <joint name="${robot_name}_joint_4" type="revolute">
      <parent link="${robot_name}_link_3"/>
      <child link="${robot_name}_link_4"/>
      <origin xyz="0 0 0.2155" rpy="${PI / 2} 0 0"/>
      <axis xyz="0 0 1"/>
      <limit lower="${-120 * PI / 180}" upper="${120 * PI / 180}"
             effort="${max_effort}" velocity="${max_velocity}" />
      <safety_controller soft_lower_limit="${-118 * PI / 180}"
                         soft_upper_limit="${118 * PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/> 
      <dynamics damping="${joint_damping}"/>
    </joint>
    
    <link name="${robot_name}_link_4">
      <inertial>
        <origin xyz="0 0.067 0.034" rpy="0 0 0"/>
        <mass value="2.7"/>
        <inertia ixx="0.03"  ixy="0"  ixz="0" iyy="0.01" iyz="0" izz="0.029" />
      </inertial>
      
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/visual/link_4.stl"/>
        </geometry>
        <material name="Orange"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/collision/link_4.stl"/>
        </geometry>
        <material name="Orange"/>
      </collision>
    </link>
    
    <!-- joint between link_4 and link_5 -->
    <joint name="${robot_name}_joint_5" type="revolute">
      <parent link="${robot_name}_link_4"/>
      <child link="${robot_name}_link_5"/>
      <origin xyz="0 0.1845 0" rpy="${-PI / 2} ${PI} 0"/>
      <axis xyz="0 0 1"/>
      <limit lower="${-170 * PI / 180}" upper="${170 * PI / 180}"
             effort="${max_effort}" velocity="${max_velocity}" />
      <safety_controller soft_lower_limit="${-168 * PI / 180}"
                         soft_upper_limit="${168 * PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/> 
      <dynamics damping="${joint_damping}"/>
    </joint>
    
    <link name="${robot_name}_link_5">
      <inertial>
        <origin xyz="0.0001 0.021 0.076" rpy="0 0 0"/>
        <mass value="1.7"/>
        <inertia ixx="0.02"  ixy="0"  ixz="0" iyy="0.018" iyz="0" izz="0.005" />
      </inertial>
      
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/visual/link_5.stl"/>
        </geometry>
        <material name="Orange"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/collision/link_5.stl"/>
        </geometry>
        <material name="Orange"/>
      </collision>
    </link>
    
    <!-- joint between link_5 and link_6 -->
    <joint name="${robot_name}_joint_6" type="revolute">
      <parent link="${robot_name}_link_5"/>
      <child link="${robot_name}_link_6"/>
      <origin xyz="0 0 0.2155" rpy="${PI / 2} 0 0"/>
      <axis xyz="0 0 1"/>
      <limit lower="${-120 * PI / 180}" upper="${120 * PI / 180}"
             effort="${max_effort}" velocity="${max_velocity}" />
      <safety_controller soft_lower_limit="${-118 * PI / 180}"
                         soft_upper_limit="${118 * PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/> 
      <dynamics damping="${joint_damping}"/>
    </joint>
    
    <link name="${robot_name}_link_6">
      <inertial>
        <origin xyz="0 0.0006 0.0004" rpy="0 0 0"/>
        <mass value="1.8"/>
        <inertia ixx="0.005"  ixy="0"  ixz="0" iyy="0.0036" iyz="0" izz="0.0047" />
      </inertial>
      
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/visual/link_6.stl"/>
        </geometry>
        <material name="Orange"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/collision/link_6.stl"/>
        </geometry>
        <material name="Orange"/>
      </collision>
    </link>
    
    <!-- joint between link_6 and link_7 -->
    <joint name="${robot_name}_joint_7" type="revolute">
      <parent link="${robot_name}_link_6"/>
      <child link="${robot_name}_link_7"/>
      <origin xyz="0 0.081 0" rpy="${- PI / 2} ${PI} 0"/>
      <axis xyz="0 0 1"/>
      <limit lower="${-175 * PI / 180}" upper="${175 * PI / 180}"
             effort="${max_effort}" velocity="${max_velocity}" />
      <safety_controller soft_lower_limit="${-173 * PI / 180}"
                         soft_upper_limit="${173 * PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/> 
      <dynamics damping="${joint_damping}"/>
    </joint>
    
    <link name="${robot_name}_link_7">
      <inertial>
        <origin xyz="0 0 0.02" rpy="0 0 0"/>
        <mass value="0.3"/>
        <inertia ixx="0.001"  ixy="0"  ixz="0" iyy="0.001" iyz="0" izz="0.001" />
      </inertial>
      
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/visual/link_7.stl"/>
        </geometry>
        <material name="Grey"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iiwa_description/meshes/iiwa14/collision/link_7.stl"/>
        </geometry>
        <material name="Grey"/>
      </collision>
    </link>
    
    <joint name="${robot_name}_joint_ee" type="fixed">
      <parent link="${robot_name}_link_7"/>
      <child link="${robot_name}_link_ee"/>
      <origin xyz="0 0 0.045" rpy="0 -${PI/2} 0"/>
    </joint>
    
    <link name="${robot_name}_link_ee">
    </link>
    
    <joint name="${robot_name}_joint_ee_kuka" type="fixed">
      <parent link="${robot_name}_link_7"/>
      <child link="${robot_name}_link_ee_kuka"/>
      <origin xyz="0 0 0.045" rpy="${PI} ${PI} ${PI}"/>
      <axis xyz="0 0 1"/>
    </joint>
    
    <link name="${robot_name}_link_ee_kuka">
    </link>
    
    <!--Extensions -->
    <xacro:iiwa_gazebo robot_name="${robot_name}" />
    <xacro:iiwa_transmission hardware_interface="${hardware_interface}"/>
    
  </xacro:macro>
  
</robot>
