<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from /home/tim/catkin_ws/src/hya_gazebo/hya_description/urdf/arm/arm.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!-- This was generated from arm.urdf.xacro in hya_gazebo/hya_description and modified to
remove the unneeded links and joints -->
<robot name="hya" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!-- Things that are needed only for Gazebo (not the physical robot). These include sensor and controller plugin specifications -->
  <!-- Inertial values -->
  <!-- Shoulder AA link -->
  <!-- Shoulder FE link -->
  <!-- Humerus R link -->
  <!-- Elbow FE link -->
  <!-- Wrist R link -->
  <!-- Wrist FE link -->
  <!-- <xacro:include filename="$(find hya_gazebo)/hya_description/urdf/arm/arm.transmission.xacro"/> -->
  <!-- Joints -->
  <!-- Note that it is assumed that the 6 arm joints are defined last! -->
  <!-- Shoulder AA joint -->
  <joint name="hya_saa_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="hya_base_link"/>
    <child link="hya_shoulder_aa_link"/>
    <axis xyz="0 0 1"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="150.0" lower="-3.1416" upper="0.5236" velocity="12.0"/>
  </joint>
  <!-- Shoulder FE joint -->
  <joint name="hya_sfe_joint" type="revolute">
    <origin rpy="1.57079632679 0 0" xyz="0 0 0.178"/>
    <parent link="hya_shoulder_aa_link"/>
    <child link="hya_shoulder_fe_link"/>
    <axis xyz="0 0 1"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="150.0" lower="-0.7679" upper="0.8552" velocity="12.0"/>
  </joint>
  <!-- Humerus R joint -->
  <joint name="hya_hr_joint" type="revolute">
    <origin rpy="0 1.57079632679 0" xyz="0.112 0 0"/>
    <parent link="hya_shoulder_fe_link"/>
    <child link="hya_humerus_r_link"/>
    <axis xyz="0 0 1"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="150.0" lower="-1.6406" upper="0.0698" velocity="12.0"/>
  </joint>
  <!-- Elbow FE joint -->
  <joint name="hya_efe_joint" type="revolute">
    <origin rpy="0 -1.57079632679 0" xyz="0 0 0.304"/>
    <parent link="hya_humerus_r_link"/>
    <child link="hya_elbow_fe_link"/>
    <axis xyz="0 0 1"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="150.0" lower="0" upper="2.2689" velocity="12.0"/>
  </joint>
  <!-- Wrist R joint -->
  <joint name="hya_wr_joint" type="revolute">
    <origin rpy="0 1.57079632679 0" xyz="0.112 0 0"/>
    <parent link="hya_elbow_fe_link"/>
    <child link="hya_wrist_r_link"/>
    <axis xyz="0 0 1"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="150.0" lower="-2.0944" upper="1.5708" velocity="12.0"/>
  </joint>
  <!-- Wrist FE joint -->
  <joint name="hya_wfe_joint" type="revolute">
    <origin xyz="0.02075 0.00000 0.18400" rpy="2.3561944901923457 -1.5707962635746238 2.3561944901923457"/>
    <parent link="hya_wrist_r_link"/>
    <child link="hya_wrist_fe_link"/>
    <axis xyz="0 0 1"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="150.0" lower="-0.5236" upper="1.5708" velocity="12.0"/>
  </joint>
  <!-- Links -->
  <!-- Heavy base link -->
  <link name="hya_base_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="1.0"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
      <!--		<origin xyz="${xcom_shoulder_aa} ${ycom_shoulder_aa} ${zcom_shoulder_aa}"/>
		<mass value="${m_shoulder_aa}"/>
		<inertia ixx="${ixx_shoulder_aa}" iyy="${iyy_shoulder_aa}" izz="${izz_shoulder_aa}"
				 ixy="${ixy_shoulder_aa}" ixz="${ixz_shoulder_aa}" iyz="${iyz_shoulder_aa}"/>-->
    </inertial>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/base.dae" scale="1 1 1"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/base.dae" scale="1 1 1"/>
      </geometry>
    </visual>
  </link>
  <!-- Shoulder AA link -->
  <link name="hya_shoulder_aa_link">
    <inertial>
      <origin xyz="-0.000196 -0.000361 0.003091"/>
      <mass value="2.6888738"/>
      <inertia ixx="0.004985" ixy="0.000585" ixz="-0.000613" iyy="0.007629" iyz="-0.000894" izz="0.004716"/>
    </inertial>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0 0.178"/>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/shoulder_aa.dae" scale="1 1 1"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0 0 0.178"/>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/shoulder_aa.dae" scale="1 1 1"/>
      </geometry>
    </visual>
  </link>
  <!-- Shoulder FE link -->
  <link name="hya_shoulder_fe_link">
    <inertial>
      <origin xyz="-0.003832 -0.023382 0.0011662"/>
      <mass value="2.5924192"/>
      <inertia ixx="0.018197" ixy="-0.002752" ixz="-0.021739" iyy="0.052659" iyz="0.001261" izz="0.039003"/>
    </inertial>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="0.112 0 0"/>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/shoulder_fe.dae" scale="1 1 1"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 1.57079632679 0" xyz="0.112 0 0"/>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/shoulder_fe.dae" scale="1 1 1"/>
      </geometry>
    </visual>
  </link>
  <!-- Humerus R link -->
  <link name="hya_humerus_r_link">
    <inertial>
      <origin xyz="-0.0023905974 0.0061804946 -0.025544674"/>
      <mass value="2.3269372"/>
      <inertia ixx="0.01033" ixy="-0.00572" ixz="-0.013406" iyy="0.030957" iyz="5.4e-05" izz="0.023452"/>
    </inertial>
    <collision>
      <origin rpy="0 -1.57079632679 0" xyz="0 0 0.304"/>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/humerus_r.dae" scale="1 1 1"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 -1.57079632679 0" xyz="0 0 0.304"/>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/humerus_r.dae" scale="1 1 1"/>
      </geometry>
    </visual>
  </link>
  <!-- Elbow FE link -->
  <link name="hya_elbow_fe_link">
    <inertial>
      <origin xyz="-0.14664831 -0.011245545 0.0010224985"/>
      <mass value="1.7423721"/>
      <inertia ixx="0.081658" ixy="-0.002292" ixz="-0.000212" iyy="0.004234" iyz="-0.00271" izz="0.081661"/>
    </inertial>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="0.112 0 0"/>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/elbow_fe.dae" scale="1 1 1"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 1.57079632679 0" xyz="0.112 0 0"/>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/elbow_fe.dae" scale="1 1 1"/>
      </geometry>
    </visual>
  </link>
  <!-- Wrist R link -->
  <link name="hya_wrist_r_link">
    <inertial>
      <origin xyz="0.00039787203 -0.0026089323 -0.023167611"/>
      <mass value="2.1032866"/>
      <inertia ixx="0.019432" ixy="0.001387" ixz="0.012652" iyy="0.029115" iyz="0.001243" izz="0.011046"/>
    </inertial>
    <collision>
      <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0.02075 0 0.184"/>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/wrist_r.dae" scale="1 1 1"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.0475"/>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/wrist_r.dae" scale="1 1 1"/>
      </geometry>
    </visual>
  </link>
  <!-- Wrist FE link -->
  <link name="hya_wrist_fe_link">
    <inertial>
      <origin xyz="-0.078762366 -0.010631221 -0.0008358458"/>
      <mass value="1.64583"/>
      <inertia ixx="0.010737" ixy="-0.000198" ixz="-1.5e-05" iyy="0.003227" iyz="0.000829" izz="0.009277"/>
    </inertial>
    <collision>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/wrist_fe.dae" scale="1 1 1"/>
      </geometry>
    </collision>
    <visual>
      <origin xyz="0.0 0.0 0.0" rpy="0 0.0 0"/>
      <geometry>
        <mesh filename="package://ct_models/urdf/HyA/meshes/wrist_fe.dae" scale="1 1 1"/>
      </geometry>
    </visual>
  </link>
+
+  <link name="hya_ee0" />
+  <joint name="hya_ee0_joint" type="fixed">
+    <parent link="hya_wrist_fe_link"/>
+    <child  link="hya_ee0"/>
+    <origin xyz="0.03 0 0"/>
+    <axis   xyz="0 0 0"/>
+    <limit lower="0" upper="0" effort="100" velocity="0.5" />
+  </joint>
+
  <!-- Gazebo extensions -->
  <!-- Links -->
  <!-- Moving platform -->
  <gazebo reference="hya_platform">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>100.0</maxVel>
    <minDepth>0.00</minDepth>
    <selfCollide>true</selfCollide>
    <material>Gazebo/Blue</material>
    <gravity>false</gravity>
  </gazebo>
  <!-- Base link -->
  <gazebo reference="hya_base_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <!-- Was 1.5 for mu1 and mu2 -->
    <mu2>0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <selfCollide>true</selfCollide>
  </gazebo>
  <!-- Shoulder AA link -->
  <gazebo reference="hya_shoulder_aa_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <selfCollide>true</selfCollide>
  </gazebo>
  <!-- Shoulder FE link -->
  <gazebo reference="hya_shoulder_fe_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <selfCollide>true</selfCollide>
  </gazebo>
  <!-- Humerus HR link -->
  <gazebo reference="hya_humerus_r_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <selfCollide>true</selfCollide>
  </gazebo>
  <!-- Elbow FE link -->
  <gazebo reference="hya_elbow_fe_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <selfCollide>true</selfCollide>
  </gazebo>
  <!-- Wrist WR link -->
  <gazebo reference="hya_wrist_r_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <selfCollide>true</selfCollide>
  </gazebo>
  <!-- Wrist FE link -->
  <gazebo reference="hya_wrist_fe_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <selfCollide>true</selfCollide>
  </gazebo>
  <!-- Plugins -->
  <gazebo>
    <plugin filename="libdisturbance_plugin.so" name="DisturbancePlugin">
      <robotName>hya</robotName>
      <amplitude>0.1</amplitude>
      <period>2.0</period>
    </plugin>
    <plugin filename="libid_plugin.so" name="IDPlugin">
      <robotName>hya</robotName>
      <SAA_init>0</SAA_init>
      <SFE_init>0</SFE_init>
      <HR_init>-0.7854</HR_init>
      <EFE_init>0.7854</EFE_init>
      <WR_init>0.7854</WR_init>
      <WFE_init>0</WFE_init>
    </plugin>
  </gazebo>
</robot>
