<?xml version="1.0"?>

<!--
  Author: Jimmy Da Silva
  Contributers: Kelsey Hawkins, Ajit Krisshna N L, Muhammad Asif Rana 

-->

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:include filename="$(find ur_description)/urdf/ur10.transmission.xacro" />
  <xacro:include filename="$(find ur_description)/urdf/ur10.gazebo.xacro" />
  <xacro:include filename="$(find ur_description)/urdf/materials.urdf.xacro" />

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

  <!-- Inertia parameters -->
  <property name="base_mass" value="4.0" />  <!-- This mass might be incorrect -->
  <property name="shoulder_mass" value="7.778" />
  <property name="upper_arm_mass" value="12.93" />
  <property name="forearm_mass" value="3.87" />
  <property name="wrist_1_mass" value="1.96" />
  <property name="wrist_2_mass" value="1.96" />
  <property name="wrist_3_mass" value="0.202" />

  <!-- 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. -->
  <property name="shoulder_cog" value="0.00008 0.00244 -0.037" />
  <property name="upper_arm_cog" value="0.00001 0.15061 0.38757" />  
  <property name="forearm_cog" value="-0.00012 0.06112 0.1984" />       
  <property name="wrist_1_cog" value="-0.00021 -0.00112 0.02269" />     
  <property name="wrist_2_cog" value="-0.00021 0.00112 0.002269" />       
  <property name="wrist_3_cog" value="0 -0.001156 -0.00149" />

  <!-- Kinematic model -->
  <property name="shoulder_height" value="0.1273" /> <!-- d1 -->  
  <property name="shoulder_offset" value="0.220941" /> <!-- Estimated -->  
  <property name="upper_arm_length" value="0.612" /> <!-- -a2 -->
  <property name="elbow_offset" value="0.163941" /> <!-- d4 -->       
  <property name="forearm_length" value="0.5723" /> <!-- -a3 -->
  <property name="wrist_1_length" value="0.1157" /> <!-- d5 -->     
  <property name="wrist_2_length" value="0.0922" /> <!-- d6 -->
  <property name="wrist_3_length" value="0.0857" /> <!-- This might be wrong -->

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

  <!-- Collision model -->
  <property name="base_collision_length" value="0.160" /> 
  <property name="shoulder_collision_length" value="0.200" />  
  <property name="shoulder_collision_offset" value="0.035" />  
  <property name="elbow_collision_length" value="0.200" />     
  <property name="elbow_collision_offset" value="0.035" />     



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

	<link name="${prefix}base_link" >
		<visual>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/visual/Base.dae" scale="${scale}"/>
		  </geometry>
		  <origin xyz="0.0 0.0 0.0" rpy="${pi / 2.0} 0.0 0.0" />
		  <material name="UR/Blue" />
		</visual>
		
		<collision>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/collision/Base.dae" scale="${scale}"/>
		  </geometry>
		  <origin xyz="0.0 0.0 0.0" rpy="${pi / 2.0} 0.0 0.0" />
		</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 0.0 1.0" />
		<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
		<dynamics damping="10" friction="0.1"/>
	</joint>
  
	<link name="${prefix}shoulder_link">
		<visual>
		  <geometry>
  		    <mesh filename="package://ur_description/meshes/ur10/visual/Shoulder.dae" scale="${scale}"/>
		  </geometry>
                  <origin rpy="${pi / 2.0} 0.0 0.0" xyz="0.0 0.0 -0.127"/>   <!-- -0.127 ????  -->
		  <material name="UR/Grey" />
		</visual>

		<collision>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/collision/Shoulder.dae" scale="${scale}"/>
		  </geometry>
		  <origin rpy="${pi / 2.0} 0.0 0.0" xyz="0.0 0.0 -0.127"/>   <!-- -0.127 ????  -->
		</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.0 1.0 0.0" />
		<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
		<dynamics damping="10" friction="0.1"/>
	</joint>

	<link name="${prefix}upper_arm_link">
		<visual>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/visual/UpperArm.dae" scale="${scale}"/>
		  </geometry>
		  <origin xyz="0.0 -0.221 -0.127" rpy="${pi / 2.0} 0.0 0.0" />  <!-- new offset-->
		  <material name="UR/Blue" />
		</visual>

		<collision>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/collision/UpperArm.dae" scale="${scale}"/>
		  </geometry>
		  <origin xyz="0.0 -0.221 -0.127" rpy="${pi / 2.0} 0.0 0.0" />  <!-- new offset-->
		</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.0 1.0 0.0" />
		<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
		<dynamics damping="10" friction="0.1"/>
	</joint>

	<link name="${prefix}forearm_link">
		<visual>
 		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/visual/Forearm.dae" scale="${scale}"/>
		  </geometry>
		  <origin xyz="0.0 -0.049 -0.739" rpy="${pi / 2.0} 0.0 0.0" />  <!-- new offset -->
		  <material name="UR/Grey" />
		</visual>

		<collision>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/collision/Forearm.dae" scale="${scale}"/>
		  </geometry>
		  <origin xyz="0.0 -0.049 -0.739" rpy="${pi / 2.0} 0.0 0.0" />  <!-- new offset -->
		</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.0 1.0 0.0" />
		<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
		<dynamics damping="10" friction="0.1"/>
	</joint>

	<link name="${prefix}wrist_1_link">
		<visual>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/visual/Wrist1.dae" scale="${scale}"/>
		  </geometry>
		  <origin xyz="0.0 -0.049 -1.312" rpy="${pi / 2.0} 0 0.0" />  <!-- new offset -->
		  <material name="UR/Blue" />
		</visual>

		<collision>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/collision/Wrist1.dae" scale="${scale}"/>
		  </geometry>
		  <origin xyz="0.0 -0.049 -1.312" rpy="${pi / 2.0} 0 0.0" />  <!-- new offset -->
		</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 0.0 1.0" />
		<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
		<dynamics damping="10" friction="0.1"/>
	</joint>

	<link name="${prefix}wrist_2_link">
		<visual>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/visual/Wrist2.dae" scale="${scale}"/>
		  </geometry>
		  <origin xyz="0.0 -0.164 -1.312" rpy="${pi / 2.0} 0.0 0.0" />  <!-- new offset -->
		  <material name="UR/Grey" />
		</visual>

		<collision>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/collision/Wrist2.dae" scale="${scale}"/>
		  </geometry>
		  <origin xyz="0.0 -0.164 -1.312" rpy="${pi / 2.0} 0.0 0.0" />  <!-- new offset -->
		</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.0 1.0 0.0" />
		<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
		<dynamics damping="10" friction="0.1"/>
	</joint>

	<link name="${prefix}wrist_3_link">
		<visual>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/visual/Wrist3.dae" scale="${scale}"/>
		  </geometry>
 		  <origin xyz="0.0 -0.164 -1.427" rpy="${pi / 2.0} 0.0 0" />   <!-- new offset -->
		  <material name="UR/Blue" />
		</visual>

		<collision>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/collision/Wrist3.dae" scale="${scale}"/>
		  </geometry>
 		  <origin xyz="0.0 -0.164 -1.427" rpy="${pi / 2.0} 0.0 0" />   <!-- new offset -->
		</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>

  <xacro:ur10_arm_transmission prefix="${prefix}" />
  <xacro:ur10_arm_gazebo prefix="${prefix}" />

  </xacro:macro>
</robot>