<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from /home/bot/catkin_ws/src/cyton_gamma_1500_description/cyton_gamma_1500.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="cyton_gamma_1500" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <link name="base_footprint">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
      <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    
    <!-- Empty collision box. For gazebo -->
    <collision>
      <origin xyz="0 0 0"/>
      <geometry>
        <box size="0 0 0"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0"/>
      <inertia
        ixx="0.0" ixy="0.0" ixz="0.0"
        iyy="0.0" iyz="0.0"
        izz="0.0"/>
    </inertial>
  </link>
  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
  </joint>
  <link name="base_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://cyton_gamma_1500_description/meshes/base.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0.0115 0.047"/>
      <geometry>
        <box size="0.154 0.177 0.094"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="0 0.0115 0.047" rpy="0 0 0"/>
      <mass value="0.4"/>
      <inertia
        ixx="0.1" ixy="0.0" ixz="0.0"
        iyy="0.1" iyz="0.0"
        izz="0.1"/>
    </inertial>
  </link>
  <joint name="shoulder_roll_joint" type="revolute">
    <parent link="base_link"/>
    <child link="shoulder_roll"/>
    <origin rpy="0 0 0" xyz="0 0 0.05315"/>
    <limit effort="100.0" lower="-2.618" upper="2.618" velocity="1.0"/>
    <axis xyz="0 0 1"/>
  </joint>
  <!-- For gazebo -->
  <transmission name="shoulder_roll_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="shoulder_roll_joint">
      <!--<hardwareInterface>EffortJointInterface</hardwareInterface>-->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="shoulder_roll_joint_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  
  <link name="shoulder_roll">
    <visual>
      <origin rpy="1.5708 0 0.384" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://cyton_gamma_1500_description/meshes/shoulder_roll.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0.0 0.0 0.10"/>
      <geometry>
        <box size="0.09 0.07 0.12"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="0 0 0.10" rpy="0 0 0"/>
      <mass value="0.4"/>
      <inertia
        ixx="0.1" ixy="0.0" ixz="0.0"
        iyy="0.1" iyz="0.0"
        izz="0.1"/>
    </inertial>
  </link>
  <joint name="shoulder_pitch_joint" type="revolute">
    <parent link="shoulder_roll"/>
    <child link="shoulder_pitch"/>
    <origin rpy="0 0 0" xyz="0.0205 0 0.12435"/>
    <limit effort="100.0" lower="-1.8326" upper="1.8326" velocity="1.0"/>
    <axis xyz="1 0 0"/>
  </joint>
  <!-- For gazebo -->
  <transmission name="shoulder_pitch_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="shoulder_pitch_joint">
      <!--<hardwareInterface>EffortJointInterface</hardwareInterface>-->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="shoulder_pitch_joint_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  
  <link name="shoulder_pitch">
    <visual>
      <origin rpy="1.5708 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://cyton_gamma_1500_description/meshes/shoulder_pitch.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="-0.02 0.0 0.1"/>
      <geometry>
        <box size="0.07 0.09 0.14"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="-0.02 0.0 0.1" rpy="0 0 0"/>
      <mass value="0.4"/>
      <inertia
        ixx="0.1" ixy="0.0" ixz="0.0"
        iyy="0.1" iyz="0.0"
        izz="0.1"/>
    </inertial>
  </link>
  <joint name="shoulder_yaw_joint" type="revolute">
    <parent link="shoulder_pitch"/>
    <child link="shoulder_yaw"/>
    <origin rpy="0 0 0" xyz="-0.0215 -0.0205 0.1255"/>
    <limit effort="100.0" lower="-1.8326" upper="1.8326" velocity="1.0"/>
    <axis xyz="0 -1 0"/>
  </joint>
  <!-- For gazebo -->
  <transmission name="shoulder_yaw_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="shoulder_yaw_joint">
      <!--<hardwareInterface>EffortJointInterface</hardwareInterface>-->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="shoulder_yaw_joint_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  
  <link name="shoulder_yaw">
    <visual>
      <origin rpy="1.5708 0 -1.5708" xyz="0 0 0.0"/>
      <geometry>
        <mesh filename="package://cyton_gamma_1500_description/meshes/shoulder_yaw.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0.0 0.02 0.10"/>
      <geometry>
        <box size="0.085 0.065 0.12"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="0 0.02 0.10" rpy="0 0 0"/>
      <mass value="0.4"/>
      <inertia
        ixx="0.1" ixy="0.0" ixz="0.0"
        iyy="0.1" iyz="0.0"
        izz="0.1"/>
    </inertial>
  </link>
  <joint name="elbow_pitch_joint" type="revolute">
    <parent link="shoulder_yaw"/>
    <child link="elbow_pitch"/>
    <origin rpy="0 0 0" xyz="0.018 0.0206 0.1158"/>
    <limit effort="100.0" lower="-1.8326" upper="1.8326" velocity="1.0"/>
    <axis xyz="1 0 0"/>
  </joint>
  <!-- For gazebo -->
  <transmission name="elbow_pitch_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="elbow_pitch_joint">
      <!--<hardwareInterface>EffortJointInterface</hardwareInterface>-->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="elbow_pitch_joint_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  
  <link name="elbow_pitch">
    <visual>
      <origin rpy="1.5708 0 3.1416" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://cyton_gamma_1500_description/meshes/elbow_pitch.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="-0.015 0.004 0.08"/>
      <geometry>
        <box size="0.055 0.0758 0.085"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="-0.015 0.005 0.09" rpy="0 0 0"/>
      <mass value="0.4"/>
      <inertia
        ixx="0.1" ixy="0.0" ixz="0.0"
        iyy="0.1" iyz="0.0"
        izz="0.1"/>
    </inertial>
  </link>
  <joint name="elbow_yaw_joint" type="revolute">
    <parent link="elbow_pitch"/>
    <child link="elbow_yaw"/>
    <origin rpy="0 0 0" xyz="-0.0171 -0.018 0.09746"/>
    <limit effort="100.0" lower="-1.8326" upper="1.8326" velocity="1.0"/>
    <axis xyz="0 -1 0"/>
  </joint>
  <!-- For gazebo -->
  <transmission name="elbow_yaw_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="elbow_yaw_joint">
      <!--<hardwareInterface>EffortJointInterface</hardwareInterface>-->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="elbow_yaw_joint_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  
  <link name="elbow_yaw">
    <visual>
      <origin rpy="1.5708 0 1.5708" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://cyton_gamma_1500_description/meshes/elbow_yaw.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0.0 0.015 0.055"/>
      <geometry>
        <box size="0.055 0.055 0.05"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="0.0 0.015 0.055" rpy="0 0 0"/>
      <mass value="0.4"/>
      <inertia
        ixx="0.1" ixy="0.0" ixz="0.0"
        iyy="0.1" iyz="0.0"
        izz="0.1"/>
    </inertial>
  </link>
  <joint name="wrist_pitch_joint" type="revolute">
    <parent link="elbow_yaw"/>
    <child link="wrist_pitch"/>
    <origin rpy="0 0 0" xyz="0.02626 0.018 0.0718"/>
    <limit effort="100.0" lower="-1.8326" upper="1.8326" velocity="1.0"/>
    <axis xyz="1 0 0"/>
  </joint>
  <!-- For gazebo -->
  <transmission name="wrist_pitch_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wrist_pitch_joint">
      <!--<hardwareInterface>EffortJointInterface</hardwareInterface>-->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="wrist_pitch_joint_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  
  <link name="wrist_pitch">
    <visual>
      <origin rpy="1.5708 0 1.5708" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://cyton_gamma_1500_description/meshes/wrist_pitch.dae"/>
      </geometry>
    </visual>
    <!-- For gazebo -->
    <collision>
      <origin xyz="0 0 0"/>
      <geometry>
        <box size="0 0 0"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.0001"/>
      <inertia
        ixx="0.1" ixy="0.0" ixz="0.0"
        iyy="0.1" iyz="0.0"
        izz="0.1"/>
    </inertial>
  </link>
  <joint name="wrist_roll_joint" type="revolute">
    <parent link="wrist_pitch"/>
    <child link="wrist_roll"/>
    <origin rpy="0 0 0" xyz="-0.026255 0 0.051425"/>
    <limit effort="100.0" lower="-2.61799" upper="2.61799" velocity="1.0"/>
    <axis xyz="0 0 1"/>
  </joint>
  <!-- For gazebo -->
  <transmission name="wrist_roll_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wrist_roll_joint">
      <!--<hardwareInterface>EffortJointInterface</hardwareInterface>-->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="wrist_roll_joint_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  
  <link name="wrist_roll">
    <visual>
      <origin rpy="3.1416 0 -1.5708" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://cyton_gamma_1500_description/meshes/wrist_roll.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0.0 0.005 0.065"/>
      <geometry>
        <box size="0.065 0.053 0.028"/>
      </geometry>

    </collision>
    <collision>
      <origin xyz="0.005 0.00 0.03"/>
      <geometry>
        <box size="0.052 0.04 0.04"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="0.0 0.005 0.065" rpy="0 0 0"/>
      <mass value="0.4"/>
      <inertia
        ixx="0.1" ixy="0.0" ixz="0.0"
        iyy="0.1" iyz="0.0"
        izz="0.1"/>
    </inertial>
  </link>
  <joint name="gripper_joint" type="prismatic">
    <parent link="wrist_roll"/>
    <child link="gripper_finger1"/>
    <origin rpy="0 0 0" xyz="-0.01331 0.00485 0.077225"/>
  <!-- For gazebo: smaller limits so that the gripper fingers do not collide. -->
    <limit effort="100.0" lower="-0.008" upper="0.005" velocity="1.0"/>
    <axis xyz="1 0 0"/>
  </joint>
  <!-- For gazebo -->
  <transmission name="gripper_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="gripper_joint">
      <!--<hardwareInterface>EffortJointInterface</hardwareInterface>-->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="gripper_joint_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  
  <link name="gripper_finger1">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://cyton_gamma_1500_description/meshes/gripper_finger1.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="-0.005 0.01 0.02"/>
      <geometry>
        <box size="0.015 0.015 0.03"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="-0.005 0.01 0.02" rpy="0 0 0"/>
      <mass value="0.4"/>
      <inertia
        ixx="0.1" ixy="0.0" ixz="0.0"
        iyy="0.1" iyz="0.0"
        izz="0.1"/>
    </inertial>
  </link>
  <joint name="gripper_joint2" type="prismatic">
    <parent link="wrist_roll"/>
    <child link="gripper_finger2"/>
    <origin rpy="0 0 0" xyz="0.0098 0.00485 0.077225"/>
  <!-- For gazebo: smaller limits so that the gripper fingers do not collide. -->    
  <limit effort="100.0" lower="-0.005" upper="0.008" velocity="1.0"/>
    <axis xyz="1 0 0"/>
  </joint>
  <!-- For gazebo -->
  <transmission name="gripper_joint2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="gripper_joint2">
      <!--<hardwareInterface>EffortJointInterface</hardwareInterface>-->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="gripper_joint2_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="gripper_finger2">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://cyton_gamma_1500_description/meshes/gripper_finger2.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0.005 0.01 0.02"/>
      <geometry>
        <box size="0.015 0.015 0.03"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="0.005 0.01 0.02" rpy="0 0 0"/>
      <mass value="0.4"/>
      <inertia
        ixx="0.1" ixy="0.0" ixz="0.0"
        iyy="0.1" iyz="0.0"
        izz="0.1"/>
    </inertial>
  </link>
  <!-- virtual end-effector -->
  <joint name="virtual_endeffector_joint" type="fixed">
    <parent link="wrist_roll"/>
    <child link="virtual_endeffector"/>
    <origin rpy="0 0 0" xyz="-0.002316 0.0079 0.079425"/>
  </joint>
  <link name="virtual_endeffector">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.02"/>
      <geometry>
        <sphere radius="0.002"/>
      </geometry>
      <material name="grey">
        <color rgba="0.7 0.7 0.7 0.8"/>
      </material>
    </visual>
  </link>
 
  <joint name="wrist_camera_joint" type="fixed">
    <parent link="wrist_roll"/>
    <child link="camera_back"/>
    <origin rpy="0 0 0" xyz="0.00 -0.0175 0.035"/>
  </joint>

 <link name="camera_back">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
  <!--<mesh filename="package://cyton_gamma_1500_description/meshes/camera/cameraBack.stl"/>  
        -->
  <sphere radius="0.002"/>
  
      </geometry>
      <material name="green">
        <color rgba="0.7 0.7 0.07 0.8"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.0 -0.0225 0.0125"/>
      <geometry>
        <box size="0.04 0.045 0.025"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="0.0 -0.0225 0.0125" rpy="0 0 0"/>
      <mass value="0.1"/>
      <inertia
        ixx="0.1" ixy="0.0" ixz="0.0"
        iyy="0.1" iyz="0.0"
        izz="0.1"/>
    </inertial>
  </link>

 <joint name="camera_box_joint" type="fixed">
    <parent link="camera_back"/>
    <child link="camera_front"/>
    <origin rpy="0 0 0" xyz="0.000 -0.035 0.025"/>
  </joint>

  <link name="camera_front">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
       <!-- <mesh filename="package://cyton_gamma_1500_description/meshes/camera/camera_front.stl"/>  
-->
  <sphere radius="0.002"/>
      </geometry>
      <material name="blue">
        <color rgba="0.7 0.07 0.7 0.8"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.0 0.005 0.0075"/>
      <geometry>
        <box size="0.055 0.03 0.025"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="0 0.005 0.0075" rpy="0 0 0"/>
      <mass value="0.1"/>
      <inertia
        ixx="0.1" ixy="0.0" ixz="0.0"
        iyy="0.1" iyz="0.0"
        izz="0.1"/>
    </inertial>
  </link>

  <!-- KINECT -->
  <link name="camera">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0" />
      <geometry>
        <box size="0.10 0.035 0.035"/>
      </geometry>
      <material name="green">
        <color rgba="0.0 1.0 0.0 1.0" />
      </material>
    </visual>
    <collision>
      <origin xyz="0.0 0.0 0.0"/>
      <geometry>
        <box size="0.10 0.035 0.035"/>
      </geometry>
    </collision>
    
    <!-- For gazebo -->
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.1"/>
      <inertia
        ixx="0.1" ixy="0.0" ixz="0.0"
        iyy="0.1" iyz="0.0"
        izz="0.1"/>
    </inertial>
  </link>

   <joint name="kinect_joint" type="fixed">
      <parent link="camera_front"/>
      <child link="camera"/>
      <origin rpy="0 0 0" xyz="0.0 -0.03 0.0"/>
    </joint>

  <!-- For gazebo: stick the robot to the ground -->
  <link name="world"/>
  <joint name="world_base" type="fixed">
    <parent link="world"/>
    <child link="base_footprint"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>

<gazebo reference="camera">  
    <sensor type="depth" name="camera">
      <visualize>1</visualize>
      <always_on>true</always_on>
      <update_rate>20.0</update_rate>
      <camera>
        <horizontal_fov>1.39626</horizontal_fov> <!-- 80 degrees -->
        <image>
          <format>R8G8B8</format>
          <width>800</width>
          <height>600</height>
        </image>
        <clip>
          <near>0.05</near>
          <far>8.0</far>
        </clip>
      </camera>
      <plugin name="kinect_controller" filename="libgazebo_ros_openni_kinect.so">
        <frameName>camera</frameName>
        <baseline>0.2</baseline>
        <alwaysOn>true</alwaysOn>
        <updateRate>1.0</updateRate>
        <cameraName>kinect_ir</cameraName>
        <imageTopicName>/image_raw</imageTopicName>
        <cameraInfoTopicName>/camera_info</cameraInfoTopicName>
        <depthImageTopicName>/depth/image_raw</depthImageTopicName>
        <depthImageCameraInfoTopicName>/depth/camera_info</depthImageCameraInfoTopicName>
        <pointCloudTopicName>/kinect_ir/points</pointCloudTopicName>
        <pointCloudCutoff>0.5</pointCloudCutoff>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
        <CxPrime>0</CxPrime>
        <Cx>0</Cx>
        <Cy>0</Cy>
        <focalLength>0</focalLength>
        <hackBaseline>0</hackBaseline>
      </plugin>
    </sensor>
  </gazebo>




 <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/cyton_arm</robotNamespace>
    </plugin>
  </gazebo>

</robot>
