<?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://ros.org/wiki/xacro">

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

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

  <!-- Inertia parameters -->
  <property name="base_mass" value="4.0" /> 
  <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" />  
  <property name="forearm_cog" value="0.0 0.0265 0.11993" />       
  <property name="wrist_1_cog" value="0.0 0.110949 0.01634" />     
  <property name="wrist_2_cog" value="0.0 0.0018 0.11099" />       
  <property name="wrist_3_cog" value="0.0 0.001159 0.0" />

  <!-- Kinematic model -->
  <property name="shoulder_height" value="0.128" />  
  <property name="shoulder_offset" value="0.1704" />  
  <property name="upper_arm_length" value="0.60186" />
  <property name="elbow_offset" value="0.12817" />       
  <property name="forearm_length" value="0.56415" />
  <property name="wrist_1_length" value="0.11279" />     
  <property name="wrist_2_length" value="0.11279" />   
  <property name="wrist_3_length" value="0.0857" />

  <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="parent name *origin">

	<!-- joint between base_link and ur_base_link -->
	<joint name="${name}_base_joint" type="fixed" >
		<insert_block name="origin" />
		<parent link="${parent}" />
		<child link="${name}_base_link" />
	</joint>

	<link name="${name}_base_link" >
		<visual>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/Base.stl" />
		  </geometry>
		  <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 ${3.0 * pi / 4.0}" />
		  <material name="UR/Blue" />
		</visual>

		<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="${name}_shoulder_pan_joint" type="revolute">
		<parent link="${name}_base_link" />
		<child link = "${name}_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="${name}_shoulder_link">
		<visual>
		  <geometry>
  		    <mesh filename="package://ur_description/meshes/ur10/Shoulder.stl" />
		  </geometry>
		  <material name="UR/Grey" />
		</visual>

		<collision>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/Shoulder.stl" />
		  </geometry>
		</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="${name}_shoulder_lift_joint" type="revolute">
		<parent link="${name}_shoulder_link" />
		<child link = "${name}_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="${name}_upper_arm_link">
		<visual>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/UpperArm.stl" />
		  </geometry>
		  <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
		  <material name="UR/Blue" />
		</visual>

		<collision>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/UpperArm.stl" />
		  </geometry>
 		  <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
		</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="${name}_elbow_joint" type="revolute">
		<parent link="${name}_upper_arm_link" />
		<child link = "${name}_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="${name}_forearm_link">
		<visual>
 		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/Forearm.stl" />
		  </geometry>
		  <material name="UR/Grey" />
		</visual>

		<collision>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/Forearm.stl" />
		  </geometry>
		</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="${name}_wrist_1_joint" type="revolute">
 		<parent link="${name}_forearm_link" />
		<child link = "${name}_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="${name}_wrist_1_link">
		<visual>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/Wrist1.stl" />
		  </geometry>
		  <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0 0.0" />
		  <material name="UR/Blue" />
		</visual>

		<collision>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/Wrist1.stl" />
		  </geometry>
		  <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0 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="${name}_wrist_2_joint" type="revolute">
		<parent link="${name}_wrist_1_link" />
		<child link = "${name}_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="${name}_wrist_2_link">
		<visual>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/Wrist2.stl" />
		  </geometry>
		  <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
		  <material name="UR/Grey" />
		</visual>

		<collision>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/Wrist2.stl" />
		  </geometry>
		  <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
		</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="${name}_wrist_3_joint" type="revolute">
		<parent link="${name}_wrist_2_link" />
		<child link = "${name}_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="${name}_wrist_3_link">
		<visual>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/Wrist3.stl" />
		  </geometry>
 		  <origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 0" />
		  <material name="UR/Blue" />
		</visual>

		<collision>
		  <geometry>
		    <mesh filename="package://ur_description/meshes/ur10/Wrist3.stl" />
		  </geometry>
		  <origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 0" />
		</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 name="${name}" />
  <xacro:ur10_arm_gazebo name="${name}" />

  </xacro:macro>
</robot>