<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">

<!--
  Author: Felix Messmer
-->

  <xacro:include filename="$(find iai_ur_description)/urdf/ur.transmission.xacro" />
  <xacro:include filename="$(find iai_ur_description)/urdf/ur.gazebo.xacro" />

  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
    <inertial>
      <mass value="${mass}" />
      <xacro:insert_block name="origin" />
      <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
        iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
        izz="${0.5 * mass * radius * radius}" />
    </inertial>
  </xacro:macro>

  <xacro:macro name="ur3_robot" params="prefix joint_limited
    shoulder_pan_lower_limit:=${-pi}    shoulder_pan_upper_limit:=${pi}
    shoulder_lift_lower_limit:=${-pi}    shoulder_lift_upper_limit:=${pi}
    elbow_joint_lower_limit:=${-pi}    elbow_joint_upper_limit:=${pi}
    wrist_1_lower_limit:=${-pi}    wrist_1_upper_limit:=${pi}
    wrist_2_lower_limit:=${-pi}    wrist_2_upper_limit:=${pi}
    wrist_3_lower_limit:=${-pi}    wrist_3_upper_limit:=${pi}
    transmission_hw_interface:=hardware_interface/PositionJointInterface
    safety_limits:=false safety_pos_margin:=0.15
    safety_k_position:=20
    kinematics_file">

    <xacro:property name="__kinematics" value="${kinematics_file['kinematics']}"/>

    <!-- Inertia parameters -->
    <xacro:property name="base_mass" value="2.0" />  <!-- This mass might be incorrect -->
    <xacro:property name="shoulder_mass" value="2.0" />
    <xacro:property name="upper_arm_mass" value="3.42" />
    <xacro:property name="forearm_mass" value="1.26" />
    <xacro:property name="wrist_1_mass" value="0.8" />
    <xacro:property name="wrist_2_mass" value="0.8" />
    <xacro:property name="wrist_3_mass" value="0.35" />

    <!-- These parameters are borrowed from the urcontrol.conf file
        but are not verified for the correct permutation.
        The permutation was guessed by looking at the UR5 parameters.
        Serious use of these parameters needs further inspection. -->
    <xacro:property name="shoulder_cog" value="0.0 -0.02 0.0" />
    <xacro:property name="upper_arm_cog" value="0.13 0.0 0.1157" />
    <xacro:property name="forearm_cog" value="0.05 0.0 0.0238" />
    <xacro:property name="wrist_1_cog" value="0.0 0.0 0.01" />
    <xacro:property name="wrist_2_cog" value="0.0 0.0 0.01" />
    <xacro:property name="wrist_3_cog" value="0.0 0.0 -0.02" />

    <!-- Arbitrary offsets for shoulder/elbow joints -->
    <xacro:property name="shoulder_offset" value="0.1198" />  <!-- measured from model -->
    <xacro:property name="elbow_offset" value="0.0275" /> <!-- measured from model -->
    <xacro:property name="upper_arm_inertia_offset" value="0.12" />  <!-- measured from model -->

    <!-- This is for compatibility reasons, as transformation between base and base_link was always 180 degree in the iai_ur_description -->
    <xacro:property name="base_correction" value="${pi}" />

    <link name="${prefix}base_link" >
      <visual>
        <origin xyz="0 0 0" rpy="0 0 ${base_correction}"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/visual/base.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 ${base_correction}"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/collision/base.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.075" length="0.038" mass="${base_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}shoulder_pan_joint" type="revolute">
      <parent link="${prefix}base_link" />
      <child link = "${prefix}shoulder_link" />
      <origin xyz="${__kinematics['shoulder']['x']} ${__kinematics['shoulder']['y']} ${__kinematics['shoulder']['z']}" rpy="${__kinematics['shoulder']['roll']} ${__kinematics['shoulder']['pitch']} ${__kinematics['shoulder']['yaw'] + base_correction}" />
      <axis xyz="0 0 1" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="2.16"/>
        <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
        </xacro:if>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}" effort="330.0" velocity="2.16"/>
        <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
        </xacro:if>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}shoulder_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/visual/shoulder.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/collision/shoulder.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.075" length="0.178" mass="${shoulder_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}shoulder_lift_joint" type="revolute">
      <parent link="${prefix}shoulder_link" />
      <child link = "${prefix}upper_arm_link" />
      <origin xyz="${__kinematics['upper_arm']['x']} ${__kinematics['upper_arm']['y']} ${__kinematics['upper_arm']['z']}" rpy="${__kinematics['upper_arm']['roll']} ${__kinematics['upper_arm']['pitch']} ${__kinematics['upper_arm']['yaw']}" />
      <axis xyz="0 0 1" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="2.16"/>
        <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
        </xacro:if>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}" effort="330.0" velocity="2.16"/>
        <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
        </xacro:if>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}upper_arm_link">
      <visual>
        <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/visual/upperarm.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/collision/upperarm.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.075" length="${-1*__kinematics['forearm']['x']}" mass="${upper_arm_mass}">
        <origin xyz="${0.5*__kinematics['forearm']['x']} 0.0 ${upper_arm_inertia_offset}" rpy="0 ${pi/2} 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}elbow_joint" type="revolute">
      <parent link="${prefix}upper_arm_link" />
      <child link = "${prefix}forearm_link" />
      <origin xyz="${__kinematics['forearm']['x']} ${__kinematics['forearm']['y']} ${__kinematics['forearm']['z']}" rpy="${__kinematics['forearm']['roll']} ${__kinematics['forearm']['pitch']} ${__kinematics['forearm']['yaw']}" />
      <axis xyz="0 0 1" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.15"/>
        <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
        </xacro:if>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}" effort="150.0" velocity="3.15"/>
        <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
        </xacro:if>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}forearm_link">
      <visual>
        <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/visual/forearm.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/collision/forearm.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.075" length="${-1*__kinematics['wrist_1']['x']}" mass="${forearm_mass}">
        <origin xyz="${0.5*__kinematics['forearm']['x']} 0.0 ${elbow_offset}" rpy="0 ${pi/2} 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}wrist_1_joint" type="revolute">
      <parent link="${prefix}forearm_link" />
      <child link = "${prefix}wrist_1_link" />
      <origin xyz="${__kinematics['wrist_1']['x']} ${__kinematics['wrist_1']['y']} ${__kinematics['wrist_1']['z']}" rpy="${__kinematics['wrist_1']['roll']} ${__kinematics['wrist_1']['pitch']} ${__kinematics['wrist_1']['yaw']}" />
      <axis xyz="0 0 1" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
        <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
        </xacro:if>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}" effort="54.0" velocity="3.2"/>
        <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
        </xacro:if>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}wrist_1_link">
      <visual>
        <origin xyz="0 0 -0.085" rpy="${pi/2} 0 0"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/visual/wrist1.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 -0.085" rpy="${pi/2} 0 0"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/collision/wrist1.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.075" length="${-1*__kinematics['wrist_2']['y']}" mass="${wrist_1_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}wrist_2_joint" type="revolute">
      <parent link="${prefix}wrist_1_link" />
      <child link = "${prefix}wrist_2_link" />
      <origin xyz="${__kinematics['wrist_2']['x']} ${__kinematics['wrist_2']['y']} ${__kinematics['wrist_2']['z']}" rpy="${__kinematics['wrist_2']['roll']} ${__kinematics['wrist_2']['pitch']} ${__kinematics['wrist_2']['yaw']}" />
      <axis xyz="0 0 1" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
        <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
        </xacro:if>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}" effort="54.0" velocity="3.2"/>
        <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
        </xacro:if>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}wrist_2_link">
      <visual>
        <origin xyz="0 0 -0.085" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/visual/wrist2.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 -0.085" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/collision/wrist2.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.075" length="${__kinematics['wrist_3']['y']}" mass="${wrist_2_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}wrist_3_joint" type="revolute">
      <parent link="${prefix}wrist_2_link" />
      <child link = "${prefix}wrist_3_link" />
      <origin xyz="${__kinematics['wrist_3']['x']} ${__kinematics['wrist_3']['y']} ${__kinematics['wrist_3']['z']}" rpy="${__kinematics['wrist_3']['roll']} ${__kinematics['wrist_3']['pitch']} ${__kinematics['wrist_3']['yaw']}" />
      <axis xyz="0 0 1" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
        <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
        </xacro:if>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" effort="54.0" velocity="3.2"/>
        <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
        </xacro:if>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}wrist_3_link">
      <visual>
        <origin xyz="0 0 -0.082" rpy="${pi/2} 0 0"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/visual/wrist3.dae"/>
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 -0.082" rpy="${pi/2} 0 0"/>
        <geometry>
          <mesh filename="package://iai_ur_description/meshes/ur3/collision/wrist3.stl"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.045" length="0.0305" mass="${wrist_3_mass}">
        <origin xyz="0.0 0.0 ${-0.0305/2}" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}ee_fixed_joint" type="fixed">
      <parent link="${prefix}wrist_3_link" />
      <child link = "${prefix}ee_link" />
      <origin xyz="0.0 0.0 0.0" rpy="0.0 ${-pi/2.0} ${pi/2.0}" />
    </joint>

    <link name="${prefix}ee_link">
      <collision>
        <geometry>
          <box size="0.01 0.01 0.01"/>
        </geometry>
        <origin rpy="0 0 0" xyz="-0.01 0 0"/>
      </collision>
    </link>

    <joint name="${prefix}wrist_3_passive_joint" type="fixed">
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <parent link="${prefix}wrist_3_link"/>
      <child link="${prefix}tool0"/>
    </joint>

    <xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" />
    <xacro:ur_arm_gazebo prefix="${prefix}" />

    <!-- ROS base_link to UR 'Base' Coordinates transform -->
    <link name="${prefix}base"/>
    <joint name="${prefix}base_link-base_fixed_joint" type="fixed">
      <!-- NOTE: this rotation is only needed as long as base_link itself is
                 not corrected wrt the real robot (ie: rotated over 180
                 degrees)
      -->
      <origin xyz="0 0 0" rpy="0 0 ${-base_correction}"/>
      <parent link="${prefix}base_link"/>
      <child link="${prefix}base"/>
    </joint>

    <!-- Frame coincident with all-zeros TCP on UR controller -->
    <link name="${prefix}tool0"/>

  </xacro:macro>
</robot>
