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

  <xacro:include filename="$(find ur_description)/urdf/ur5.transmission.xacro" />
  <xacro:include filename="$(find ur_description)/urdf/ur5.gazebo.xacro" />
  <xacro:include filename="$(find ur_description)/urdf/materials.urdf.xacro" />
<!--
DH for UR5:
a = [0.00000, -0.42500, -0.39225,  0.00000,  0.00000,  0.0000]
d = [0.089159,  0.00000,  0.00000,  0.10915,  0.09465,  0.0823]
alpha = [ 1.570796327, 0, 0, 1.570796327, -1.570796327, 0 ]
q_home_offset = [0, -1.570796327, 0, -1.570796327, 0, 0]
joint_direction = [-1, -1, 1, 1, 1, 1]
mass = [3.7000, 8.3930, 2.2750, 1.2190, 1.2190, 0.1879]
center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0, 0.0265], [0, -0.0018, 0.01634], [0, 0.0018,0.01634], [0, 0, -0.001159] ]
-->

  <property name="pi" value="3.14159265" />

<!-- Inertia parameters -->
  <property name="base_mass" value="4.0" /> <!-- Invented number, only matters for simulator -->
  <property name="shoulder_mass" value="3.7000" />
  <property name="upper_arm_mass" value="8.3930" />
  <property name="forearm_mass" value="2.2750" />
  <property name="wrist_1_mass" value="1.2190" />
  <property name="wrist_2_mass" value="1.2190" />
  <property name="wrist_3_mass" value="0.1879" />

  <property name="shoulder_cog" value="0.0 0.00193 -0.02561" />
  <property name="upper_arm_cog" value="0.0 -0.024201 0.2125" />   <!-- 0.11336 - 0.089159 =  -->
  <property name="forearm_cog" value="0.0 0.0265 0.11993" />       <!-- 0.119 is not half of 0.39225, is this really correct? -->
  <property name="wrist_1_cog" value="0.0 0.110949 0.01634" />     <!-- 0.0018 + 0.10915 = 0.110949 -->
  <property name="wrist_2_cog" value="0.0 0.0018 0.11099" />       <!-- 0.01634 + 0.09465 = 0.11099-->
  <property name="wrist_3_cog" value="0.0 0.001159 0.0" />

<!-- Kinematic model -->
  <property name="shoulder_height" value="0.089159" />  
  <property name="shoulder_offset" value="0.13585" />  <!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 -->
  <property name="upper_arm_length" value="0.42500" />
  <property name="elbow_offset" value="0.1197" />       <!-- CAD measured -->
  <property name="forearm_length" value="0.39225" />
  <property name="wrist_1_length" value="0.093" />     <!-- CAD measured -->
  <property name="wrist_2_length" value="0.09465" />   <!-- In CAD this distance is 0.930, but in the spec it is 0.09465 -->
  <property name="wrist_3_length" value="0.0823" />

  <property name="shoulder_radius" value="0.060" />   <!-- manually measured -->
  <property name="upper_arm_radius" value="0.054" />  <!-- manually measured -->
  <property name="elbow_radius" value="0.060" />      <!-- manually measured -->
  <property name="forearm_radius" value="0.040" />    <!-- manually measured -->
  <property name="wrist_radius" value="0.045" />      <!-- manually measured -->

<xacro:macro name="ur5_robot" params="prefix">

  <link name="${prefix}base_link" >
    <visual>
      <geometry>
		    <mesh filename="package://ur_description/meshes/ur5/visual/Base.dae" />
      </geometry>
      <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 ${3.0 * pi / 4.0}" />
    </visual>
	<collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/Base.dae" />
      </geometry>
      <!-- <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.009159"/> -->
    </collision>
    <inertial>
      <mass value="${base_mass}" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>

  <joint name="${prefix}shoulder_pan_joint" type="revolute">
    <parent link="${prefix}base_link" />
    <child link = "${prefix}shoulder_link" />
    <origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" />
    <axis xyz="0 0 1" />
    <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="${pi}"/>
    <dynamics damping="0.4" friction="0.0"/>
  </joint>
  
  <link name="${prefix}shoulder_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/Shoulder.dae" />
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/Shoulder.dae" />
      </geometry>
      <!-- <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.009159"/> -->
    </collision>
    <inertial>
      <mass value="${shoulder_mass}" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      <origin xyz="${shoulder_cog}" rpy="0.0 0.0 0.0" />
    </inertial>
  </link>

  <joint name="${prefix}shoulder_lift_joint" type="revolute">
    <parent link="${prefix}shoulder_link" />
    <child link = "${prefix}upper_arm_link" />
    <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />    
    <axis xyz="0 1 0" />
    <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="${pi}"/>
    <dynamics damping="0.4" friction="0.0"/>
  </joint>

  <link name="${prefix}upper_arm_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/UpperArm.dae" />
      </geometry>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/UpperArm.dae" />
      </geometry>
<!--       <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.2125"/> -->
    </collision>
    <inertial>
      <mass value="${upper_arm_mass}" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      <origin xyz="${upper_arm_cog}" rpy="0.0 0.0 0.0" />
    </inertial>
  </link>

  <joint name="${prefix}elbow_joint" type="revolute">
    <parent link="${prefix}upper_arm_link" />
    <child link = "${prefix}forearm_link" />
    <origin xyz="0.0 ${-elbow_offset} ${upper_arm_length}" rpy="0.0 0.0 0.0" />
    <axis xyz="0 1 0" />
    <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="${pi}"/>
    <dynamics damping="0.4" friction="0.0"/>
  </joint>

  <link name="${prefix}forearm_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/Forearm.dae" />
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/Forearm.dae" />
      </geometry>
<!--       <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.196125"/> -->
    </collision>
    <inertial>
      <mass value="${forearm_mass}" />
      <origin xyz="${forearm_cog}" rpy="0.0 0.0 0.0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>

  <joint name="${prefix}wrist_1_joint" type="revolute">
    <parent link="${prefix}forearm_link" />
    <child link = "${prefix}wrist_1_link" />
    <origin xyz="0.0 0.0 ${forearm_length}" rpy="0.0 ${pi / 2.0} 0.0" />
    <axis xyz="0 1 0" />
    <limit lower="${-1.0 * pi}" upper="${0.25 * pi}" effort="28.0" velocity="${pi}"/>
    <dynamics damping="0.3" friction="0.0"/>
  </joint>

  <link name="${prefix}wrist_1_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/Wrist1.dae" />
      </geometry>
      <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/Wrist1.dae" />
      </geometry>
      <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.093 0.0"/>
    </collision>
    <inertial>
      <mass value="${wrist_1_mass}" />
      <origin xyz="${wrist_1_cog}" rpy="0.0 0.0 0.0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>

  <joint name="${prefix}wrist_2_joint" type="revolute">
    <parent link="${prefix}wrist_1_link" />
    <child link = "${prefix}wrist_2_link" />
    <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0.0 0.0" />
    <axis xyz="0 0 1" />
    <limit lower="${-1.0 * pi}" upper="${1.0 * pi}" effort="28.0" velocity="${pi}"/>
    <dynamics damping="0.3" friction="0.0"/>
  </joint>

  <link name="${prefix}wrist_2_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/Wrist2.dae" />
      </geometry>
      <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/Wrist2.dae" />
      </geometry>
       <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/> 
    </collision>
    <inertial>
      <mass value="${wrist_2_mass}" />
      <origin xyz="${wrist_2_cog}" rpy="0.0 0.0 0.0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>

  <joint name="${prefix}wrist_3_joint" type="revolute">
    <parent link="${prefix}wrist_2_link" />
    <child link = "${prefix}wrist_3_link" />
    <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
    <axis xyz="0 1 0" />
    <limit lower="${-1.0 * pi}" upper="${1.0 * pi}" effort="28.0" velocity="${pi}"/>
    <dynamics damping="0.3" friction="0.0"/>
  </joint>

  <link name="${prefix}wrist_3_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/Wrist3.dae" />
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/Wrist3.dae" />
      </geometry>
    </collision>
    <inertial>
      <mass value="${wrist_3_mass}" />
      <origin xyz="${wrist_3_cog}" rpy="0.0 0.0 0.0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </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 ${wrist_3_length} 0.0" rpy="0.0 0.0 ${pi/2.0}" />
  </joint>

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

  <xacro:ur5_arm_transmission prefix="${prefix}" />
  <xacro:ur5_arm_gazebo prefix="${prefix}" />

  </xacro:macro>
</robot>