<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  
  <!-- Import all Gazebo-customization elements, including Gazebo colors -->
  <xacro:include filename="$(find iai_iiwa_description)/urdf/iiwa.gazebo.xacro" />
  <!-- Import Transmissions -->
  <xacro:include filename="$(find iai_iiwa_description)/urdf/iiwa.transmission.xacro" />
  <!-- Include Utilities -->
  <xacro:include filename="$(find iai_iiwa_description)/urdf/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="iiwa7" 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://iai_iiwa_description/meshes/iiwa7/visual/link_0.stl"/>
        </geometry>
        <material name="Grey"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/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.15" 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="3.4525"/>
        <inertia ixx="0.02183"  ixy="0"  ixz="0" iyy="0.007703" iyz="-0.003887" izz="0.02083" />
      </inertial>
      <visual>
        <origin xyz="0 0 0.0075" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/visual/link_1.stl"/>
        </geometry>
        <material name="Orange"/>
      </visual>
      <collision>
        <origin xyz="0 0 0.0075" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/collision/link_1.stl"/>
        </geometry>
        <material name="Orange"/>
      </collision>
    </link>
    
    <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.19" 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="3.4821"/>
        <inertia ixx="0.02076"  ixy="0"  ixz="-0.003626" iyy="0.02179" iyz="0" izz="0.00779" />
      </inertial>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/visual/link_2.stl"/>
        </geometry>
        <material name="Orange"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/collision/link_2.stl"/>
        </geometry>
        <material name="Orange"/>
      </collision>
    </link>
    
    <joint name="${robot_name}_joint_3" type="revolute">
      <parent link="${robot_name}_link_2"/>
      <child link="${robot_name}_link_3"/>
      <origin xyz="0 0.21 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="4.05623"/>
        <inertia ixx="0.03204"  ixy="0"  ixz="0" iyy="0.00972" iyz="0.006227" izz="0.03042" />
      </inertial>
      
      <visual>
        <origin xyz="0 0 -0.026" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/visual/link_3.stl"/>
        </geometry>
        <material name="Orange"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 -0.026" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/collision/link_3.stl"/>
        </geometry>
        <material name="Orange"/>
      </collision>
      
    </link>
    
    <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.19" 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="3.4822"/>
        <inertia ixx="0.02178"  ixy="0"  ixz="0" iyy="0.02075" iyz="-0.003625" izz="0.007785" />
      </inertial>
      
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/visual/link_4.stl"/>
        </geometry>
        <material name="Orange"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/collision/link_4.stl"/>
        </geometry>
        <material name="Orange"/>
      </collision>
    </link>
    
    <joint name="${robot_name}_joint_5" type="revolute">
      <parent link="${robot_name}_link_4"/>
      <child link="${robot_name}_link_5"/>
      <origin xyz="0 0.21 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="2.1633"/>
        <inertia ixx="0.01287"  ixy="0"  ixz="0" iyy="0.005708" iyz="-0.003946" izz="0.01112" />
      </inertial>
      
      <visual>
        <origin xyz="0 0 -0.026" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/visual/link_5.stl"/>
        </geometry>
        <material name="Orange"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 -0.026" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/collision/link_5.stl"/>
        </geometry>
        <material name="Orange"/>
      </collision>
    </link>
    
    <joint name="${robot_name}_joint_6" type="revolute">
      <parent link="${robot_name}_link_5"/>
      <child link="${robot_name}_link_6"/>
      <origin xyz="0 0.06070 0.19" 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="2.3466"/>
        <inertia ixx="0.006509"  ixy="0"  ixz="0" iyy="0.006259" iyz="0.00031891" izz="0.004527" />
      </inertial>
      
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/visual/link_6.stl"/>
        </geometry>
        <material name="Orange"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/collision/link_6.stl"/>
        </geometry>
        <material name="Orange"/>
      </collision>
    </link>
    
    <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.06070" 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="3.129"/>
        <inertia ixx="0.01464"  ixy="0.0005912"  ixz="0" iyy="0.01465" iyz="0" izz="0.002872" />
      </inertial>
      
      <visual>
        <origin xyz="0 0 -0.0005" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/visual/link_7.stl"/>
        </geometry>
        <material name="Grey"/>
      </visual>
      
      <collision>
        <origin xyz="0 0 -0.0005" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_iiwa_description/meshes/iiwa7/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 0 0"/>
    </joint>
    
    <link name="${robot_name}_link_ee">
    </link>
    
    <!--Extensions -->
    <xacro:iiwa_gazebo robot_name="${robot_name}" />
    <xacro:iiwa_transmission hardware_interface="${hardware_interface}"/>
    
  </xacro:macro>
  
</robot>
