<?xml version="1.0"?>
<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
       xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:include filename="$(find iai_kuka_lwr4_description)/defs/util_defs.xml"/>

  <xacro:property name="arm_elem_base_mass" value="2.0"/>
  <xacro:property name="arm_elem_link_mass" value="2.0"/>
  <xacro:property name="arm_elem_ball_link_mass" value="2.0"/>
  <xacro:property name="arm_elem_end_link_mass" value="2.0"/>
  <xacro:property name="arm_elem_flange_link_mass" value="0.5"/>
  <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.1" />

  <xacro:property name="arm_velocity_scale_factor" value="1"/>

  <xacro:macro name="kuka_lwr_arm" params="parent prefix *origin color_postfix">

    <joint name="${parent}_${prefix}arm_base_joint" type="fixed">
      <xacro:insert_block name="origin"/>
      <parent link="${parent}"/>
      <child link="calib_${prefix}arm_base_link"/>
    </joint>

    <link name="calib_${prefix}arm_base_link">
      <inertial>
        <mass value="${arm_elem_base_mass}"/>
        <origin xyz="0 0 0.055" rpy="0 0 0"/>
        <cylinder_inertia_def radius="0.06" length="0.11"
                              mass="${arm_elem_base_mass}"/>
      </inertial>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <!-- for each mesh we have two versions, one Collada file with normals and colors, and one simpler STL. The collada file is in cm, and the STL in meters -->
          <mesh filename="package://iai_kuka_lwr4_description/meshes/arm_base${color_postfix}.dae" scale="1 1 1"/>
	  <!--mesh filename="package://iai_kuka_lwr4_description/meshes/arm_base.stl" scale="1 1 1"-->
        </geometry>
        <!--material name="UniHB-Red"/-->
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/convex/arm_base_convex.stl"/>
        </geometry>
      </collision>
    </link>

    <gazebo reference="calib_${prefix}arm_base_link">
      <material value="kuka-lwr.material"/>
    </gazebo>

    <joint name="${prefix}arm_0_joint" type="revolute">
      <origin xyz="0 0 0.11" rpy="0 0 0"/>
      <axis xyz="0 0 1"/>
      <limit lower="${-150 * M_PI / 180}" upper="${150 * M_PI / 180}"
             effort="204" velocity="${arm_velocity_scale_factor * 110 * M_PI / 180}" />
      <safety_controller soft_lower_limit="${-148 * M_PI / 180}"
                         soft_upper_limit="${148 * M_PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/>
      <dynamics damping="${joint_damping}"/>
      <calibration rising="0.0"/>
      <parent link="calib_${prefix}arm_base_link"/>
      <child link="${prefix}arm_1_link"/>
    </joint>

    <link name="${prefix}arm_1_link">
      <inertial>
        <mass value="${arm_elem_link_mass}"/>
        <origin rpy="0 0 0" xyz="0 0 0.130"/>
        <cuboid_inertia_def length="0.12" width="0.06" height="0.260"
                            mass="${arm_elem_link_mass}"/>
      </inertial>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/arm_segment_a${color_postfix}.dae" scale="1 1 1"/>
          <!--mesh filename="package://iai_kuka_lwr4_description/meshes/arm_segment_a.stl" scale="1 1 1"/-->
        </geometry>
        <!--material name="UniHB-Red"/-->
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/convex/arm_segment_a_convex.stl"/>
        </geometry>
      </collision>
    </link>

    <gazebo reference="${prefix}arm_1_link">
      <material value="kuka-lwr.material"/>
    </gazebo>

    <transmission name="${prefix}arm_0_trans" type="pr2_mechanism_model/SimpleTransmission">
      <actuator name="${prefix}arm_0_motor"/>
      <joint name="${prefix}arm_0_joint"/>
      <mechanicalReduction>1.0</mechanicalReduction>
    </transmission>

    <joint name="${prefix}arm_1_joint" type="revolute">
      <origin xyz="0 0 0.20" rpy="0 0 0"/>
      <axis xyz="0 -1 0"/>
      <limit lower="${-107 * M_PI / 180}" upper="${107 * M_PI / 180}"
             effort="306" velocity="${arm_velocity_scale_factor * 110 * M_PI / 180}" />
      <safety_controller soft_lower_limit="${-105 * M_PI / 180}"
                         soft_upper_limit="${105 * M_PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/>
      <dynamics damping="${joint_damping}"/>
      <calibration rising="0.0"/>
      <parent link="${prefix}arm_1_link"/>
      <child link="${prefix}arm_2_link"/>
    </joint>

    <link name="${prefix}arm_2_link">
      <inertial>
        <mass value="${arm_elem_link_mass}"/>
        <origin rpy="0 0 0" xyz="0 0.06 ${0.130 - 0.06}"/>
        <cuboid_inertia_def length="0.12" width="0.06" height="0.260"
                            mass="${arm_elem_link_mass}"/>
      </inertial>

      <visual>
        <origin xyz="0 0 0.2" rpy="${M_PI} 0 ${M_PI}"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/arm_segment_b${color_postfix}.dae" scale="1 1 1"/>
          <!--mesh filename="package://iai_kuka_lwr4_description/meshes/arm_segment_b.stl" scale="1 1 1"/-->
        </geometry>
        <!--material name="UniHB-Red"/-->
      </visual>

      <collision>
        <origin xyz="0 0 0.2" rpy="${M_PI} 0 ${M_PI}"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/convex/arm_segment_b_convex.stl"/>
        </geometry>
      </collision>
    </link>

    <gazebo reference="${prefix}arm_2_link">
      <material value="kuka-lwr.material"/>
    </gazebo>

    <transmission name="${prefix}arm_1_trans" type="pr2_mechanism_model/SimpleTransmission">
      <actuator name="${prefix}arm_1_motor"/>
      <joint name="${prefix}arm_1_joint"/>
      <mechanicalReduction>1.0</mechanicalReduction>
    </transmission>

    <joint name="${prefix}arm_2_joint" type="revolute">
      <origin xyz="0 0 0.20" rpy="0 0 0"/>
      <axis xyz="0 0 1"/>
      <limit lower="${-150 * M_PI / 180}" upper="${150 * M_PI / 180}"
             effort="204" velocity="${arm_velocity_scale_factor * 130 * M_PI / 180}" />
      <safety_controller soft_lower_limit="${-148 * M_PI / 180}"
                         soft_upper_limit="${148 * M_PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/>
      <dynamics damping="${joint_damping}"/>
      <calibration rising="0.0"/>
      <parent link="${prefix}arm_2_link"/>
      <child link="${prefix}arm_3_link"/>
    </joint>

    <link name="${prefix}arm_3_link">
      <inertial>
        <mass value="${arm_elem_link_mass}"/>
        <origin rpy="0 0 0" xyz="0 0.06 0.130"/>
        <cuboid_inertia_def length="0.12" width="0.06" height="0.260"
                            mass="${arm_elem_link_mass}"/>
      </inertial>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/arm_segment_a${color_postfix}.dae" scale="1 1 1"/>
          <!--mesh filename="package://iai_kuka_lwr4_description/meshes/arm_segment_a.stl" scale="1 1 1"/-->
        </geometry>
        <!--material name="UniHB-Red"/-->
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/convex/arm_segment_a_convex.stl"/>
        </geometry>
      </collision>
    </link>

    <gazebo reference="${prefix}arm_3_link">
      <material value="kuka-lwr.material"/>
    </gazebo>

    <transmission name="${prefix}arm_2_trans" type="pr2_mechanism_model/SimpleTransmission">
      <actuator name="${prefix}arm_2_motor"/>
      <joint name="${prefix}arm_2_joint"/>
      <mechanicalReduction>1.0</mechanicalReduction>
    </transmission>

    <joint name="${prefix}arm_3_joint" type="revolute">
      <origin xyz="0 0 0.20" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
      <limit lower="${-107 * M_PI / 180}" upper="${107 * M_PI / 180}"
             effort="306" velocity="${arm_velocity_scale_factor * 130 * M_PI / 180}" />
      <safety_controller soft_lower_limit="${-105 * M_PI / 180}"
                         soft_upper_limit="${105 * M_PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/>
      <dynamics damping="${joint_damping}"/>
      <calibration rising="0.0"/>
      <parent link="${prefix}arm_3_link"/>
      <child link="${prefix}arm_4_link"/>
    </joint>

    <link name="${prefix}arm_4_link">
      <inertial>
        <mass value="${arm_elem_link_mass}"/>
        <origin rpy="0 0 0" xyz="0 -0.06 ${0.130 - 0.06}"/>
        <cuboid_inertia_def length="0.12" width="0.06" height="0.2600"
                            mass="${arm_elem_link_mass}"/>
      </inertial>

      <visual>
        <origin xyz="0 0 0.2" rpy="0 ${M_PI} ${M_PI}"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/arm_segment_b${color_postfix}.dae" scale="1 1 1"/>
          <!--mesh filename="package://iai_kuka_lwr4_description/meshes/arm_segment_b.stl" scale="1 1 1"/-->
        </geometry>
        <!--material name="UniHB-Red"/-->
      </visual>

      <collision>
        <origin xyz="0 0 0.2" rpy="0 ${M_PI} ${M_PI}"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/convex/arm_segment_b_convex.stl"/>
        </geometry>
      </collision>
    </link>

    <gazebo reference="${prefix}arm_4_link">
      <material value="kuka-lwr.material"/>
    </gazebo>

    <transmission name="${prefix}arm_3_trans" type="pr2_mechanism_model/SimpleTransmission">
      <actuator name="${prefix}arm_3_motor"/>
      <joint name="${prefix}arm_3_joint"/>
      <mechanicalReduction>1.0</mechanicalReduction>
    </transmission>

    <joint name="${prefix}arm_4_joint" type="revolute">
      <origin xyz="0 0 0.20" rpy="0 0 0"/>
      <axis xyz="0 0 1"/>
      <limit lower="${-150 * M_PI / 180}" upper="${150 * M_PI / 180}"
             effort="204" velocity="${arm_velocity_scale_factor * 130 * M_PI / 180}" />
      <safety_controller soft_lower_limit="${-148 * M_PI / 180}"
                         soft_upper_limit="${148 * M_PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/>
      <dynamics damping="${joint_damping}"/>
      <calibration rising="0.0"/>
      <parent link="${prefix}arm_4_link"/>
      <child link="${prefix}arm_5_link"/>
    </joint>

    <link name="${prefix}arm_5_link">
      <inertial>
        <mass value="${arm_elem_link_mass}"/>
        <origin rpy="0 0 0" xyz="0 0 0.124"/>
        <cuboid_inertia_def length="0.12" width="0.06" height="0.248"
                            mass="${arm_elem_link_mass}"/>
      </inertial>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
        <geometry name="${prefix}arm_5_geom">
          <mesh filename="package://iai_kuka_lwr4_description/meshes/arm_segment_last${color_postfix}.dae" scale="1 1 1"/> 
          <!--mesh filename="package://iai_kuka_lwr4_description/meshes/arm_segment_last.stl" scale="1 1 1"/-->
        </geometry>
        <!--material name="UniHB-Red"/-->
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/convex/arm_segment_last_convex.stl"/>
        </geometry>
      </collision>
    </link>

    <gazebo reference="${prefix}arm_5_link">
      <material value="kuka-lwr.material"/>
    </gazebo>

    <transmission name="${prefix}arm_4_trans" type="pr2_mechanism_model/SimpleTransmission">
      <actuator name="${prefix}arm_4_motor"/>
      <joint name="${prefix}arm_4_joint"/>
      <mechanicalReduction>1.0</mechanicalReduction>
    </transmission>

    <joint name="${prefix}arm_5_joint" type="revolute">
      <origin xyz="0 0 0.19" rpy="0 0 0"/>
      <axis xyz="0 -1 0"/>
      <limit lower="${-115 * M_PI / 180}" upper="${115 * M_PI / 180}"
             effort="306" velocity="${arm_velocity_scale_factor * 180 * M_PI / 180}" />
      <safety_controller soft_lower_limit="${-113 * M_PI / 180}"
                         soft_upper_limit="${113 * M_PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/>
      <dynamics damping="${joint_damping}"/>
      <calibration rising="0.0"/>
      <parent link="${prefix}arm_5_link"/>
      <child link="${prefix}arm_6_link"/>
    </joint>

    <link name="${prefix}arm_6_link">
      <inertial>
        <mass value="${arm_elem_ball_link_mass}"/>
        <origin rpy="0 0 0" xyz="0 0 0.0625"/>
        <cuboid_inertia_def length="0.125" width="0.125" height="0.125"
                            mass="${arm_elem_ball_link_mass}"/>
      </inertial>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/arm_wrist.dae" scale="1 1 1"/>
          <!--mesh filename="package://iai_kuka_lwr4_description/meshes/arm_wrist.stl" scale="1 1 1"/-->
        </geometry>
        <material name="Grey"/>
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/convex/arm_wrist_convex.stl"/>
        </geometry>
      </collision>
    </link>

    <gazebo reference="${prefix}arm_6_link">
      <material value="kuka-lwr.material"/>
    </gazebo>

    <transmission name="${prefix}arm_5_trans" type="pr2_mechanism_model/SimpleTransmission">
      <actuator name="${prefix}arm_5_motor"/>
      <joint name="${prefix}arm_5_joint"/>
      <mechanicalReduction>1.0</mechanicalReduction>
    </transmission>

    <joint name="${prefix}arm_6_joint" type="revolute">
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <axis xyz="0 0 1"/>
      <limit lower="${-150 * M_PI / 180}" upper="${150 * M_PI / 180}"
             effort="204" velocity="${arm_velocity_scale_factor * 180 * M_PI / 180}" />
      <safety_controller soft_lower_limit="${-148 * M_PI / 180}"
                         soft_upper_limit="${148 * M_PI / 180}"
                         k_position="${safety_controller_k_pos}"
                         k_velocity="${safety_controller_k_vel}"/>
      <dynamics damping="${joint_damping}"/>
      <calibration rising="0.0"/>
      <parent link="${prefix}arm_6_link"/>
      <child link="${prefix}arm_7_link"/>
    </joint>

    <link name="${prefix}arm_7_link">
      <inertial>
        <mass value="${arm_elem_end_link_mass}"/>
        <origin xyz="0 0 0"/>
        <cuboid_inertia_def length="1" width="1" height="1"
                            mass="${arm_elem_end_link_mass}"/>
      </inertial>
      <visual>
        <origin xyz="0 0 0.078" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/arm_flanche.dae" scale="1 1 1"/>
          <!--mesh filename="package://iai_kuka_lwr4_description/meshes/arm_flanche.stl" scale="1 1 1"/-->
        </geometry>
        <material name="Black"/>
      </visual>
      <collision>
        <origin xyz="0 0 0.078" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_kuka_lwr4_description/meshes/convex/arm_flanche_convex.stl"/>
        </geometry>
      </collision>
    </link>

    <gazebo reference="${prefix}arm_7_link">
      <material value="kuka-lwr.material"/>
    </gazebo>

    <transmission name="${prefix}arm_6_trans" type="pr2_mechanism_model/SimpleTransmission">
      <actuator name="${prefix}arm_6_motor"/>
      <joint name="${prefix}arm_6_joint"/>
      <mechanicalReduction>1.0</mechanicalReduction>
    </transmission>

    <!--arm_7_link has the same pose as the TCP defined in the DLR Beasty controller:
       Center of the potato, with z-coming out towards the flange, and y going towards joint #6 -->

    <joint name="${prefix}arm_flange_joint" type="fixed">
      <origin xyz="0 0 0.078" rpy="0 0 0"/>
      <parent link="${prefix}arm_7_link"/>
      <child link="${prefix}arm_flange_link"/>
    </joint>

    <link name="${prefix}arm_flange_link">
      <inertial>
        <mass value="${arm_elem_flange_link_mass}"/>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <cuboid_inertia_def length="1" width="1" height="1"
                            mass="${arm_elem_flange_link_mass}"/>
      </inertial>

    </link>

  </xacro:macro>

</robot>
