<robot
  name="TheKraken">
  <link
    name="base">
    <inertial>
      <origin
        xyz="-1.41978772233704E-17 -0.0181187646331409 0.15"
        rpy="0 0 0" />
      <mass
        value="60" />
      <inertia
        ixx="31.0948643981295"
        ixy="-5.56214489718239E-15"
        ixz="-1.03006876930356E-14"
        iyy="39.3781323742997"
        iyz="-1.48402876174263"
        izz="13.9679619816116" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/base.STL" />  
      </geometry>
      <material
        name="">
        <color
          rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/base.STL" />
      </geometry>
    </collision>
  </link>
  <link
    name="front_left_wheel">
    <inertial>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <mass
        value="0.741059688180091" />
      <inertia
        ixx="0.00294331585874921"
        ixy="-2.83859161066948E-20"
        ixz="-4.08094699234997E-21"
        iyy="0.00152723740598811"
        iyz="2.71050543121376E-19"
        izz="0.00152723740598811" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/front_left_wheel.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.298039215686275 0.298039215686275 0.298039215686275 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/front_left_wheel.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="front_left_axel"
    type="continuous">
    <origin
      xyz="-0.523 0.2286 0.0381"
      rpy="-1.9625927389844 0 3.141592" />
    <parent
      link="base" />
    <child
      link="front_left_wheel" />
    <axis
      xyz="1 0 0" />
    <dynamics damping="2" friction="0.4"/>
  </joint>
  <link
    name="rear_left_wheel">
    <inertial>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <mass
        value="0.741059688180091" />
      <inertia
        ixx="0.00294331585874921"
        ixy="5.21952266301851E-22"
        ixz="-4.71203852915774E-23"
        iyy="0.00152723740598811"
        iyz="0"
        izz="0.00152723740598811" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/rear_left_wheel.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.298039215686275 0.298039215686275 0.298039215686275 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/rear_left_wheel.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="rear_left_axel"
    type="continuous">
    <origin
      xyz="-0.523 -0.127 0.0381"
      rpy="2.44622762298683 0 -3.97359708169347E-16" />
    <parent
      link="base" />
    <child
      link="rear_left_wheel" />
    <axis
      xyz="-1 0 0" />
    <dynamics damping="0.9" friction="0.4"/>
  </joint>
  <link
    name="front_right_wheel">
    <inertial>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <mass
        value="0.741059688180091" />
      <inertia
        ixx="0.00294331585874921"
        ixy="1.17783321979146E-20"
        ixz="5.1813333450466E-21"
        iyy="0.00152723740598811"
        iyz="3.79470760369927E-19"
        izz="0.00152723740598811" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/front_right_wheel.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.298039215686275 0.298039215686275 0.298039215686275 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/front_right_wheel.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="front_right_axel"
    type="continuous">
    <origin
      xyz="0.523 0.2286 0.0381"
      rpy="1.17899991460539 0 3.14159265358979" />
    <parent
      link="base" />
    <child
      link="front_right_wheel" />
    <axis
      xyz="-1 0 0" />
    <dynamics damping="1" friction="0.4"/>
  </joint>
  <link
    name="rear_right_wheel">
    <inertial>
      <origin
        xyz="1.11022302462516E-16 0 0"
        rpy="0 0 0" />
      <mass
        value="0.741059688180091" />
      <inertia
        ixx="0.00294331585874921"
        ixy="-2.68914227197932E-20"
        ixz="-4.09356716039033E-21"
        iyy="0.00152723740598811"
        iyz="2.71050543121376E-19"
        izz="0.00152723740598811" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/rear_right_wheel.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.298039215686275 0.298039215686275 0.298039215686275 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/rear_right_wheel.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="rear_right_axel"
    type="continuous">
    <origin
      xyz="0.523 -0.127 0.0381"
      rpy="1.17899991460539 0 3.14159265358979" />
    <parent
      link="base" />
    <child
      link="rear_right_wheel" />
    <axis
      xyz="-1 0 0" />
    <dynamics damping="1" friction="0.4"/>
  </joint>
  <link
    name="elevator">
    <inertial>
      <origin
        xyz="2.6371138290925E-12 0.0487243732108274 0.00359699106783967"
        rpy="0 0 0" />
      <mass
        value="1.28926675273714" />
      <inertia
        ixx="0.00147028886830941"
        ixy="-1.21135161760902E-14"
        ixz="6.10474663359621E-14"
        iyy="0.0347692564454485"
        iyz="4.36113447364265E-05"
        izz="0.034110157738842" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/elevator.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/elevator.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="elevator_axel"
    type="prismatic">
    <origin
      xyz="0 -0.152399999995851 0.1"
      rpy="0 0 0" />
    <parent
      link="base" />
    <child
      link="elevator" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="0"
      upper="1.66"
      effort="300"
      velocity="5"
      />
    <dynamics friction="0" damping="100"/>      
  </joint>
  <link
    name="left_claw">
    <inertial>
      <origin
        xyz="-5.55111512312578E-17 0.258632700093989 0"
        rpy="0 0 0" />
      <mass
        value="0.370467287015463" />
      <inertia
        ixx="0.0102728264423497"
        ixy="-9.15550782890233E-19"
        ixz="5.58768790212806E-19"
        iyy="4.01162187905667E-05"
        iyz="4.5997378350362E-19"
        izz="0.010272545336041" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/left_claw.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/left_claw.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="left_claw_axel"
    type="prismatic">
    <origin
      xyz="-0.252259516039193 0.0635000024608519 0.012699996798972"
      rpy="-3.1 -3.60822483003168E-16 -3.14159265358979" />
    <parent
      link="elevator" />
    <child
      link="left_claw" />
    <axis
      xyz="-1 0 0" />
     <limit
      lower="-.02"
      upper=".21"
      effort="100"
      velocity="5" />
      <dynamics damping="0.4" friction="1"/>
  </joint>
  <link
    name="right_claw">
    <inertial>
      <origin
        xyz="-2.77555756156289E-17 0.258632700093989 -5.55111512312578E-17"
        rpy="0 0 0" />
      <mass
        value="0.370467287015463" />
      <inertia
        ixx="0.0102728264423497"
        ixy="-1.07333990242552E-18"
        ixz="-2.75539016991301E-21"
        iyy="4.01162187905668E-05"
        iyz="-2.48939570183281E-19"
        izz="0.010272545336041" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/right_claw.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://TheKraken/meshes/right_claw.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="right_claw_axel"
    type="prismatic">
    <origin
      xyz="0.241523405511254 0.0635000024392046 0.0126999967967711"
      rpy="-3.1 1.63757896132211E-15 -3.14159265358979" />
    <parent
      link="elevator" />
    <child
      link="right_claw" />
    <axis
      xyz="-1 0 0" />
    <limit
      lower="-.2"
      upper="0.02"
      effort="100"
      velocity="5" />
    <dynamics damping="1" friction="1"/>
  </joint>
  <link name="Camera">
    <visual>
      <geometry>
        <box size="0.05 0.05 0.1"/>
      </geometry>
    </visual>
  </link>
  <joint
    name="CameraJoint"
    type="fixed">
    <origin
      xyz="0 -0.12 1.8"
      rpy="0 1 1.5707"/>
    <parent link="base"/>
    <child link="Camera"/>
  </joint>

  <gazebo>
    <plugin filename="libclock.so" name="clock">
      <topic>/gazebo/frc/time</topic>
    </plugin>

    <plugin filename="libgyro.so" name="chassis_gyro">
      <link>base</link>
      <topic>/gazebo/frc/simulator/analog/1</topic>
      <axis>yaw</axis>
      <units>degrees</units>
    </plugin>

    <plugin filename="libdc_motor.so" name="front_left_drive">
      <joint>front_left_axel</joint>
      <topic>/gazebo/frc/simulator/pwm/1</topic>
      <multiplier>-16</multiplier>
    </plugin>

    <plugin filename="libencoder.so" name="front_left_encoder">
      <joint>front_left_axel</joint>
      <topic>/gazebo/frc/simulator/dio/2/3</topic>
      <units>degrees</units>
    </plugin>

    <plugin filename="libdc_motor.so" name="front_right_drive">
      <joint>front_right_axel</joint>
      <topic>/gazebo/frc/simulator/pwm/2</topic>
      <multiplier>-16</multiplier>
    </plugin>

     <plugin filename="libencoder.so" name="front_right_encoder">
      <joint>front_right_axel</joint>
      <topic>/gazebo/frc/simulator/dio/4/5</topic>
      <units>degrees</units>
    </plugin>

    <plugin filename="libdc_motor.so" name="rear_left_drive">
      <joint>rear_left_axel</joint>
      <topic>/gazebo/frc/simulator/pwm/3</topic>
      <multiplier>-16</multiplier>
    </plugin>

    <plugin filename="libdc_motor.so" name="rear_right_drive">
      <joint>rear_right_axel</joint>
      <topic>/gazebo/frc/simulator/pwm/4</topic>
      <multiplier>-16</multiplier>
    </plugin>

    <plugin filename="libdc_motor.so" name="elavator_motor">
      <joint>elevator_axel</joint>
      <topic>/gazebo/frc/simulator/pwm/6</topic>
      <multiplier>175.0</multiplier>
    </plugin>

    <plugin filename="libdc_motor.so" name="left_claw_motor">
      <joint>left_claw_axel</joint>
      <topic>/gazebo/frc/simulator/pwm/5</topic>
      <multiplier>60.0</multiplier>
    </plugin>

    <plugin filename="libdc_motor.so" name="right_claw_motor">
      <joint>right_claw_axel</joint>
      <topic>/gazebo/frc/simulator/pwm/5</topic>
      <multiplier>-60.0</multiplier>
    </plugin>

    <plugin filename="libencoder.so" name="elevator_encoder">
      <joint>elevator_axel</joint>
      <topic>/gazebo/frc/simulator/dio/0/1</topic>
      <units>degrees</units>
    </plugin>

    <plugin filename="libencoder.so" name="claw_encoder">
      <joint>left_claw_axel</joint>
      <topic>/gazebo/frc/simulator/dio/6/7</topic>
      <units>degrees</units>
    </plugin>

    <plugin name="leftClawContact_limit" filename="liblimit_switch.so">
      <topic>/gazebo/frc/simulator/dio/8</topic>
      <type>external</type>
      <sensor>leftClawContact</sensor>
    </plugin>

    <plugin name="elevatorUltrasonic_rangefinder" filename="librangefinder.so">
      <sensor>elevatorUltrasonic</sensor>
      <topic>/gazebo/frc/simulator/analog/2</topic>
    </plugin>
  </gazebo>

  <gazebo reference="base">
    <sensor name="elevatorUltrasonic" type="sonar">
      <always_on>true</always_on>
      <visualize>true</visualize>
      <pose>0 0 .05 1.5707 0 0</pose>
      <sonar>
        <min>0</min>
        <max>2</max>
        <radius>0.1</radius>
      </sonar>
    </sensor>
  </gazebo>

  <gazebo reference="Camera">
    <material>Gazebo/Red</material>
    <sensor name="robotCamera" type="camera">
      <visualize>true</visualize>
      <camera>
        <horizontal_fov>1.5</horizontal_fov>
      </camera>
    </sensor>
  </gazebo>

  <gazebo reference="base">
  </gazebo>

  <gazebo reference="right_claw">
    <material>Gazebo/Green</material>
    <mu1>2</mu1>
    <mu2>2</mu2>
    <kp>100000.0</kp>
    <kd>1.0</kd>
  </gazebo>

  <gazebo reference="left_claw">
    <material>Gazebo/Green</material>
    <mu1>2</mu1>
    <mu2>2</mu2>
    <kp>100000.0</kp>
    <kd>1.0</kd>
    <sensor name="leftClawContact" type="contact">
      <always_on>true</always_on>
      <contact>
        <collision>leftClaw_collision</collision>
      </contact>
    </sensor>
  </gazebo>

  <gazebo reference="front_right_wheel">
    <material>Gazebo/Black</material>
  </gazebo>

  <gazebo reference="front_left_wheel">
    <material>Gazebo/Black</material>
  </gazebo>

  <gazebo reference="rear_right_wheel">
    <material>Gazebo/Black</material>
  </gazebo>

  <gazebo reference="rear_left_wheel">
    <material>Gazebo/Black</material>
  </gazebo>

</robot>
