Thesis-Hector-VR/Hector_v2/Assets/Asterix/asterix_model.urdf

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from /home/yannic/Dokumente/urdf/asterix_ugv.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="asterix_ugv">
  <!-- Base link gazebo properties TODO should this be chassis link?-->
  <gazebo reference="base_link">
    <mu1>0.0</mu1>
    <mu2>0.0</mu2>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <minDepth>0.001</minDepth>
    <maxVel>1.0</maxVel>
  </gazebo>
  <gazebo>
    <!-- Publish ground truth -->
    <plugin filename="libgazebo_ros_p3d.so" name="p3d_base_controller">
      <alwaysOn>true</alwaysOn>
      <updateRate>50.0</updateRate>
      <bodyName>base_link</bodyName>
      <topicName>ground_truth/state</topicName>
      <gaussianNoise>0.01</gaussianNoise>
      <frameName>world</frameName>
      <xyzOffsets>0 0 0</xyzOffsets>
      <rpyOffsets>0 0 0</rpyOffsets>
    </plugin>
    <!-- IMU plugin -->
    <plugin filename="libhector_gazebo_ros_imu.so" name="imu_controller">
      <alwaysOn>true</alwaysOn>
      <updateRate>50.0</updateRate>
      <bodyName>base_link</bodyName>
      <topicName>imu_quat</topicName>
      <accelDrift>0.0 0.0 0.0</accelDrift>
      <accelGaussianNoise>0.1 0.1 0.1</accelGaussianNoise>
      <rateDrift>0.0 0.0 0.0</rateDrift>
      <rateGaussianNoise>0.05 0.05 0.05</rateGaussianNoise>
      <headingDrift>0.0</headingDrift>
      <headingGaussianNoise>0.05</headingGaussianNoise>
    </plugin>
    <!-- Gazebo ros control for flipper joints -->
    <plugin filename="libgazebo_ros_control_select_joints.so" name="gazebo_ros_control_select_joints">
      <robotNamespace>flipper_control</robotNamespace>
      <joints>flipper_front_joint flipper_back_joint</joints>
    </plugin>
  </gazebo>
  <!-- Measured from CAD file -->
  <!--  <xacro:arg name="add_self_filter_geom" default="0"/>-->
  <!--<xacro:property name="add_self_filter_geom" value="${arg add_self_filter_geom}"/>-->
  <!-- Measured from CAD file -->
  <material name="black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>
  <material name="blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>
  <material name="green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>
  <material name="grey">
    <color rgba="0.5 0.5 0.5 1.0"/>
  </material>
  <material name="orange">
    <color rgba="1.0 0.4235294117647059 0.0392156862745098 1.0"/>
  </material>
  <material name="brown">
    <color rgba="0.8705882352941177 0.8117647058823529 0.7647058823529411 1.0"/>
  </material>
  <material name="red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  </material>
  <material name="white">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>
  <!-- Measured from CAD file -->
  <link name="base_link"/>
  <joint name="chassis_joint" type="fixed">
    <parent link="base_link"/>
    <child link="chassis_link"/>
  </joint>
  <link name="chassis_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 -0.063"/>
      <mass value="15"/>
      <inertia ixx="0.151065" ixy="0.0" ixz="0.0" iyy="0.49262625" iyz="0.0" izz="0.60400125"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://asterix_description/meshes/asterix_chassis.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="MatteMetalicSlateBlue">
        <color rgba="0 0.18 0.32 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.063"/>
      <geometry>
        <box size="0.615 0.324 0.126"/>
      </geometry>
      <material name="MatteMetalicSlateBlue"/>
    </collision>
  </link>
  <joint name="main_track_left_fixed_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.16941 0.0535 "/>
    <parent link="chassis_link"/>
    <child link="main_track_left_link"/>
  </joint>
  <link name="main_track_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0 "/>
      <mass value="2.65"/>
      <inertia ixx="0.013095416666666667" ixy="0.0" ixz="0.0" iyy="0.06557336666666666" iyz="0.0" izz="0.05530461666666666"/>
    </inertial>
    <!-- ==== Visuals ====-->
    <visual>
      <geometry>
        <mesh filename="package://asterix_description/meshes/asterix_main_track.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="LightBlack">
        <color rgba="0.1 0.1 0.1 1"/>
      </material>
    </visual>
    <!-- ==== Collision geom ==== -->
    <!-- Standard track geom -->
    <collision>
      <geometry>
        <box size="0.494 0.08 0.23"/>
      </geometry>
    </collision>
    <!-- Front wheel -->
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0.247 0 0"/>
      <geometry>
        <cylinder length="0.08" radius="0.115"/>
      </geometry>
    </collision>
    <!-- Back wheel -->
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="-0.247 0 0"/>
      <geometry>
        <cylinder length="0.08" radius="0.115"/>
      </geometry>
    </collision>
  </link>
  <joint name="main_track_right_fixed_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 -0.16941 0.0535 "/>
    <parent link="chassis_link"/>
    <child link="main_track_right_link"/>
  </joint>
  <link name="main_track_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0 "/>
      <mass value="2.65"/>
      <inertia ixx="0.013095416666666667" ixy="0.0" ixz="0.0" iyy="0.06557336666666666" iyz="0.0" izz="0.05530461666666666"/>
    </inertial>
    <!-- ==== Visuals ====-->
    <visual>
      <geometry>
        <mesh filename="package://asterix_description/meshes/asterix_main_track.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="LightBlack">
        <color rgba="0.1 0.1 0.1 1"/>
      </material>
    </visual>
    <!-- ==== Collision geom ==== -->
    <!-- Standard track geom -->
    <collision>
      <geometry>
        <box size="0.494 0.08 0.23"/>
      </geometry>
    </collision>
    <!-- Front wheel -->
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0.247 0 0"/>
      <geometry>
        <cylinder length="0.08" radius="0.115"/>
      </geometry>
    </collision>
    <!-- Back wheel -->
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="-0.247 0 0"/>
      <geometry>
        <cylinder length="0.08" radius="0.115"/>
      </geometry>
    </collision>
  </link>
  <!-- front is either 1 (for front flipper) or -1 (for back flipper) -->
  <joint name="flipper_front_joint" type="revolute">
    <origin rpy="0 3.141592653589793 3.141592653589793" xyz="0.247 0.0 0.0535"/>
    <axis xyz="0 1 0"/>
    <limit effort="1000" lower="-3.141592653589793" upper="3.141592653589793" velocity="1"/>
    <parent link="chassis_link"/>
    <child link="flipper_front_motor_link"/>
  </joint>
  <link name="flipper_front_motor_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="1"/>
      <inertia ixx="0.041666666666666664" ixy="0.0" ixz="0.0" iyy="0.041666666666666664" iyz="0.0" izz="0.041666666666666664"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0 0.0 0.0"/>
      <geometry>
        <cylinder length="0.01" radius="0.01"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0 0.0 0.0"/>
      <geometry>
        <cylinder length="0.01" radius="0.01"/>
      </geometry>
    </collision>
  </link>
  <!-- mirror is either 1 (no mirror) or -1 (mirror)  -->
  <joint name="flipper_front_left_joint" type="fixed">
    <parent link="flipper_front_motor_link"/>
    <child link="flipper_front_left_link"/>
    <origin rpy="3.141592653589793 1.5707963267948966 0" xyz="0 -0.23411 0"/>
  </joint>
  <link name="flipper_front_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2.55"/>
      <inertia ixx="0.0117725" ixy="0.0" ixz="0.0" iyy="0.0224825" iyz="0.0" izz="0.0117725"/>
    </inertial>
    <!-- TODO inertia__-->
    <!-- <inertial> <origin xyz="0.086 -0.005 0" rpy="0 0 0"/> <mass value="1.5"/> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /> </inertial>-->
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://asterix_description/meshes/asterix_flipper.stl" scale="0.001 0.001 -0.001 "/>
      </geometry>
      <material name="LightBlack">
        <color rgba="0.1 0.1 0.1 1"/>
      </material>
    </visual>
    <!-- Main wheel -->
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.115"/>
      </geometry>
    </collision>
    <!-- Track top -->
    <collision>
      <origin rpy="0 0.14 0" xyz="0.15744 0 0.0625"/>
      <geometry>
        <box size="0.26 0.05 0.06"/>
      </geometry>
    </collision>
    <!-- Track bottom -->
    <collision>
      <origin rpy="0 -0.14 0" xyz="0.15744 0 -0.0625"/>
      <geometry>
        <box size="0.26 0.05 0.06"/>
      </geometry>
    </collision>
    <!-- Small wheel -->
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0.28396 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.075"/>
      </geometry>
    </collision>
  </link>
  <!-- mirror is either 1 (no mirror) or -1 (mirror)  -->
  <joint name="flipper_front_right_joint" type="fixed">
    <parent link="flipper_front_motor_link"/>
    <child link="flipper_front_right_link"/>
    <origin rpy="3.141592653589793 1.5707963267948966 0" xyz="0 0.23411 0"/>
  </joint>
  <link name="flipper_front_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2.55"/>
      <inertia ixx="0.0117725" ixy="0.0" ixz="0.0" iyy="0.0224825" iyz="0.0" izz="0.0117725"/>
    </inertial>
    <!-- TODO inertia__-->
    <!-- <inertial> <origin xyz="0.086 -0.005 0" rpy="0 0 0"/> <mass value="1.5"/> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /> </inertial>-->
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://asterix_description/meshes/asterix_flipper.stl" scale="0.001 0.001 0.001 "/>
      </geometry>
      <material name="LightBlack">
        <color rgba="0.1 0.1 0.1 1"/>
      </material>
    </visual>
    <!-- Main wheel -->
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.115"/>
      </geometry>
    </collision>
    <!-- Track top -->
    <collision>
      <origin rpy="0 0.14 0" xyz="0.15744 0 0.0625"/>
      <geometry>
        <box size="0.26 0.05 0.06"/>
      </geometry>
    </collision>
    <!-- Track bottom -->
    <collision>
      <origin rpy="0 -0.14 0" xyz="0.15744 0 -0.0625"/>
      <geometry>
        <box size="0.26 0.05 0.06"/>
      </geometry>
    </collision>
    <!-- Small wheel -->
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0.28396 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.075"/>
      </geometry>
    </collision>
  </link>
  <!--Needed for gazebo-->
  <transmission name="flipper_front_joint_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="flipper_front_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="flipper_front_joint_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- front is either 1 (for front flipper) or -1 (for back flipper) -->
  <joint name="flipper_back_joint" type="revolute">
    <origin rpy="0 3.141592653589793 0" xyz="-0.247 0.0 0.0535"/>
    <axis xyz="0 1 0"/>
    <limit effort="1000" lower="-3.141592653589793" upper="3.141592653589793" velocity="1"/>
    <parent link="chassis_link"/>
    <child link="flipper_back_motor_link"/>
  </joint>
  <link name="flipper_back_motor_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="1"/>
      <inertia ixx="0.041666666666666664" ixy="0.0" ixz="0.0" iyy="0.041666666666666664" iyz="0.0" izz="0.041666666666666664"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0 0.0 0.0"/>
      <geometry>
        <cylinder length="0.01" radius="0.01"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0 0.0 0.0"/>
      <geometry>
        <cylinder length="0.01" radius="0.01"/>
      </geometry>
    </collision>
  </link>
  <!-- mirror is either 1 (no mirror) or -1 (mirror)  -->
  <joint name="flipper_back_left_joint" type="fixed">
    <parent link="flipper_back_motor_link"/>
    <child link="flipper_back_left_link"/>
    <origin rpy="3.141592653589793 1.5707963267948966 0" xyz="0 0.23411 0"/>
  </joint>
  <link name="flipper_back_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2.55"/>
      <inertia ixx="0.0117725" ixy="0.0" ixz="0.0" iyy="0.0224825" iyz="0.0" izz="0.0117725"/>
    </inertial>
    <!-- TODO inertia__-->
    <!-- <inertial> <origin xyz="0.086 -0.005 0" rpy="0 0 0"/> <mass value="1.5"/> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /> </inertial>-->
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://asterix_description/meshes/asterix_flipper.stl" scale="0.001 0.001 0.001 "/>
      </geometry>
      <material name="LightBlack">
        <color rgba="0.1 0.1 0.1 1"/>
      </material>
    </visual>
    <!-- Main wheel -->
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.115"/>
      </geometry>
    </collision>
    <!-- Track top -->
    <collision>
      <origin rpy="0 0.14 0" xyz="0.15744 0 0.0625"/>
      <geometry>
        <box size="0.26 0.05 0.06"/>
      </geometry>
    </collision>
    <!-- Track bottom -->
    <collision>
      <origin rpy="0 -0.14 0" xyz="0.15744 0 -0.0625"/>
      <geometry>
        <box size="0.26 0.05 0.06"/>
      </geometry>
    </collision>
    <!-- Small wheel -->
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0.28396 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.075"/>
      </geometry>
    </collision>
  </link>
  <!-- mirror is either 1 (no mirror) or -1 (mirror)  -->
  <joint name="flipper_back_right_joint" type="fixed">
    <parent link="flipper_back_motor_link"/>
    <child link="flipper_back_right_link"/>
    <origin rpy="3.141592653589793 1.5707963267948966 0" xyz="0 -0.23411 0"/>
  </joint>
  <link name="flipper_back_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2.55"/>
      <inertia ixx="0.0117725" ixy="0.0" ixz="0.0" iyy="0.0224825" iyz="0.0" izz="0.0117725"/>
    </inertial>
    <!-- TODO inertia__-->
    <!-- <inertial> <origin xyz="0.086 -0.005 0" rpy="0 0 0"/> <mass value="1.5"/> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /> </inertial>-->
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://asterix_description/meshes/asterix_flipper.stl" scale="0.001 0.001 -0.001 "/>
      </geometry>
      <material name="LightBlack">
        <color rgba="0.1 0.1 0.1 1"/>
      </material>
    </visual>
    <!-- Main wheel -->
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.115"/>
      </geometry>
    </collision>
    <!-- Track top -->
    <collision>
      <origin rpy="0 0.14 0" xyz="0.15744 0 0.0625"/>
      <geometry>
        <box size="0.26 0.05 0.06"/>
      </geometry>
    </collision>
    <!-- Track bottom -->
    <collision>
      <origin rpy="0 -0.14 0" xyz="0.15744 0 -0.0625"/>
      <geometry>
        <box size="0.26 0.05 0.06"/>
      </geometry>
    </collision>
    <!-- Small wheel -->
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0.28396 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.075"/>
      </geometry>
    </collision>
  </link>
  <!--Needed for gazebo-->
  <transmission name="flipper_back_joint_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="flipper_back_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="flipper_back_joint_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo reference="main_track_left_fixed_joint">
    <preserveFixedJoint>true</preserveFixedJoint>
  </gazebo>
  <gazebo reference="main_track_right_fixed_joint">
    <preserveFixedJoint>true</preserveFixedJoint>
  </gazebo>
  <gazebo reference="flipper_front_left_joint">
    <preserveFixedJoint>true</preserveFixedJoint>
  </gazebo>
  <gazebo reference="flipper_front_right_joint">
    <preserveFixedJoint>true</preserveFixedJoint>
  </gazebo>
  <gazebo reference="flipper_back_left_joint">
    <preserveFixedJoint>true</preserveFixedJoint>
  </gazebo>
  <gazebo reference="flipper_back_right_joint">
    <preserveFixedJoint>true</preserveFixedJoint>
  </gazebo>
  <gazebo reference="chassis_link">
    <visual>
      <material>
        <ambient>0 0.18 0.32 1</ambient>
        <diffuse>0 0.18 0.32 1</diffuse>
        <specular>0.1 0.1 0.1 1</specular>
        <emissive>0 0 0 0</emissive>
      </material>
    </visual>
  </gazebo>
  <gazebo reference="main_track_left_link">
    <!-- <mu1>0.0</mu1> <mu2>0.0</mu2> <kp>1000000.0</kp> <kd>100.0</kd> <minDepth>0.001</minDepth> <maxVel>1.0</maxVel>-->
    <material>Gazebo/FlatBlack</material>
  </gazebo>
  <gazebo reference="flipper_front_left_link">
    <!-- <mu1>0.0</mu1> <mu2>0.0</mu2> <kp>1000000.0</kp> <kd>100.0</kd> <minDepth>0.001</minDepth> <maxVel>1.0</maxVel>-->
    <material>Gazebo/FlatBlack</material>
  </gazebo>
  <gazebo reference="flipper_back_left_link">
    <!-- <mu1>0.0</mu1> <mu2>0.0</mu2> <kp>1000000.0</kp> <kd>100.0</kd> <minDepth>0.001</minDepth> <maxVel>1.0</maxVel>-->
    <material>Gazebo/FlatBlack</material>
  </gazebo>
  <gazebo reference="main_track_right_link">
    <!-- <mu1>0.0</mu1> <mu2>0.0</mu2> <kp>1000000.0</kp> <kd>100.0</kd> <minDepth>0.001</minDepth> <maxVel>1.0</maxVel>-->
    <material>Gazebo/FlatBlack</material>
  </gazebo>
  <gazebo reference="flipper_front_right_link">
    <!-- <mu1>0.0</mu1> <mu2>0.0</mu2> <kp>1000000.0</kp> <kd>100.0</kd> <minDepth>0.001</minDepth> <maxVel>1.0</maxVel>-->
    <material>Gazebo/FlatBlack</material>
  </gazebo>
  <gazebo reference="flipper_back_right_link">
    <!-- <mu1>0.0</mu1> <mu2>0.0</mu2> <kp>1000000.0</kp> <kd>100.0</kd> <minDepth>0.001</minDepth> <maxVel>1.0</maxVel>-->
    <material>Gazebo/FlatBlack</material>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_tracked_vehicle_plugin.so" name="multi_tracked_vehicle">
      <command_topic>/cmd_vel_raw</command_topic>
      <body>base_link</body>
      <left_track0>main_track_left_link</left_track0>
      <left_track1>flipper_front_left_link</left_track1>
      <left_track2>flipper_back_left_link</left_track2>
      <right_track0>main_track_right_link</right_track0>
      <right_track1>flipper_front_right_link</right_track1>
      <right_track2>flipper_back_right_link</right_track2>
      <track_mu>0.7</track_mu>
      <track_mu2>150</track_mu2>
      <tracks_separation>0.49882000000000004</tracks_separation>
      <steering_efficiency>0.5</steering_efficiency>
      <max_linear_speed>1.0</max_linear_speed>
      <max_angular_speed>1.0</max_angular_speed>
    </plugin>
    <plugin filename="libkeys_to_cmd_vel_plugin.so" name="keyboard_control">
      <cmd_vel_topic>~/robot_description_gazebo/cmd_vel</cmd_vel_topic>
    </plugin>
  </gazebo>
  <joint name="autonomy_box_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.1085 0 0.149"/>
    <parent link="chassis_link"/>
    <child link="autonomy_box_link"/>
  </joint>
  <!-- Computer box and cage -->
  <link name="autonomy_box_link">
    <inertial>
      <mass value="0.1"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="4.900000000000002e-05" ixy="0.0" ixz="0.0" iyy="4.900000000000002e-05" iyz="0.0" izz="4.900000000000002e-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="-0.095 -0.095 0"/>
      <geometry>
        <mesh filename="package://hector_components_description/meshes/autonomy_box/visionbox_no_lidar.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0595"/>
      <geometry>
        <box size="0.19 0.19 0.119"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
    <!--Protection vertical profiles-->
    <collision>
      <origin rpy="0 0 0" xyz="0.085 0.085 0.279"/>
      <geometry>
        <!--<box size="${alu_profile_side_length} ${alu_profile_side_length} ${top_protection_bottom-computer_box_height}"/>-->
        <cylinder length="0.32" radius="0.02"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="-0.085 0.085 0.279"/>
      <geometry>
        <!--<box size="${alu_profile_side_length} ${alu_profile_side_length} ${top_protection_bottom-computer_box_height}"/>-->
        <cylinder length="0.32" radius="0.02"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="-0.085 -0.085 0.279"/>
      <geometry>
        <!--<box size="${alu_profile_side_length} ${alu_profile_side_length} ${top_protection_bottom-computer_box_height}"/>-->
        <cylinder length="0.32" radius="0.02"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.085 -0.085 0.279"/>
      <geometry>
        <!--<box size="${alu_profile_side_length} ${alu_profile_side_length} ${top_protection_bottom-computer_box_height}"/>-->
        <cylinder length="0.32" radius="0.02"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
    <!--Top geom-->
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0.085 0.449"/>
      <geometry>
        <!--<box size="${computer_box_length} ${alu_profile_side_length} ${alu_profile_side_length}"/>-->
        <cylinder length="0.19" radius="0.02"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="0 -0.085 0.449"/>
      <geometry>
        <!--<box size="${computer_box_length} ${alu_profile_side_length} ${alu_profile_side_length}"/>-->
        <cylinder length="0.19" radius="0.02"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
    <collision>
      <origin rpy="1.5707963267948966 1.5707963267948966 0" xyz="0.085 0 0.449"/>
      <geometry>
        <!--<box size="${alu_profile_side_length} ${computer_box_width}  ${alu_profile_side_length}"/>-->
        <cylinder length="0.19" radius="0.02"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
    <collision>
      <origin rpy="1.5707963267948966 1.5707963267948966 0" xyz="-0.085 0 0.449"/>
      <geometry>
        <!--<box size="${alu_profile_side_length} ${computer_box_width}  ${alu_profile_side_length}"/>-->
        <cylinder length="0.19" radius="0.02"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
    <!-- 360 cam mount -->
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.5135000000000001"/>
      <geometry>
        <!--<box size="${computer_box_length} ${alu_profile_side_length} ${alu_profile_side_length}"/>-->
        <cylinder length="0.10899999999999996" radius="0.02"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
  </link>
  <!-- IMU -->
  <joint name="imu_link_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="autonomy_box_link"/>
    <child link="imu_link"/>
  </joint>
  <link name="imu_link">     
    </link>
  <joint name="spin_lidar_joint_fixed" type="fixed">
    <origin rpy="0.0 0.0 0.0" xyz="0 0 0.33865"/>
    <parent link="autonomy_box_link"/>
    <child link="spin_lidar_mount_link_fixed"/>
  </joint>
  <link name="spin_lidar_mount_link_fixed">
    </link>
  <joint name="spin_lidar_spin_joint" type="continuous">
    <origin rpy="0.0 0.0 0.0" xyz="0 0 0.33865"/>
    <parent link="autonomy_box_link"/>
    <child link="spin_lidar_mount_link"/>
    <axis xyz=" 0 0 1"/>
    <limit effort="1000" velocity="100"/>
    <joint_properties damping="500.0" friction="200.0"/>
  </joint>
  <link name="spin_lidar_mount_link">
    <visual>
      <origin rpy="0 0 3.141592653589793" xyz="0 0 -0.21803"/>
      <geometry>
        <mesh filename="package://hector_components_description/meshes/autonomy_box/visionbox_lidar_mount_only.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.1"/>
      <inertia ixx="9.000000000000002e-06" ixy="0.0" ixz="0.0" iyy="9.000000000000002e-06" iyz="0.0" izz="9.000000000000002e-06"/>
    </inertial>
  </link>
  <joint name="spin_lidar_laser_joint" type="fixed">
    <origin rpy="-2.35147 -3.13295 3.14155" xyz="0 -0.00396994 0"/>
    <parent link="spin_lidar_mount_link"/>
    <child link="spin_lidar_laser_frame"/>
  </joint>
  <link name="spin_lidar_laser_frame">
    <inertial>
      <mass value="0.83"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.0008856838700000001" ixy="0.0" ixz="0.0" iyy="0.0008856838700000001" iyz="0.0" izz="0.0008856838700000001"/>
    </inertial>
    <visual>
      <origin rpy="1.5707963267948966 0 3.141592653589793" xyz="0 0 -0.0378"/>
      <geometry>
        <mesh filename="package://hector_sensors_description/meshes/vlp16/VLP16_base_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <visual>
      <origin rpy="1.5707963267948966 0 3.141592653589793" xyz="0 0 -0.0378"/>
      <geometry>
        <mesh filename="package://hector_sensors_description/meshes/vlp16/VLP16_base_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <visual>
      <origin rpy="1.5707963267948966 0 3.141592653589793" xyz="0 0 -0.0378"/>
      <geometry>
        <mesh filename="package://hector_sensors_description/meshes/vlp16/VLP16_scan.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.00225"/>
      <geometry>
        <cylinder length="0.0717" radius="0.0516"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="spin_lidar_laser_frame">
    <sensor name="spin_lidar_laser-VLP16" type="ray">
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>10</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>512</samples>
            <resolution>1</resolution>
            <min_angle>-3.141592653589793</min_angle>
            <max_angle>3.141592653589793</max_angle>
          </horizontal>
          <vertical>
            <samples>8</samples>
            <resolution>1</resolution>
            <min_angle>-0.2617993877991494</min_angle>
            <max_angle>0.2617993877991494</max_angle>
          </vertical>
        </scan>
        <range>
          <min>0.1</min>
          <max>100</max>
          <resolution>0.001</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.0</stddev>
        </noise>
      </ray>
      <plugin filename="libgazebo_ros_velodyne_laser.so" name="gazebo_ros_laser_controller">
        <topicName>/spin_lidar/vlp16</topicName>
        <frameName>spin_lidar_laser_frame</frameName>
        <min_range>0.1</min_range>
        <max_range>100</max_range>
        <gaussianNoise>0.008</gaussianNoise>
      </plugin>
    </sensor>
  </gazebo>
  <transmission name="spin_lidar_spin_joint_joint_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="spin_lidar_spin_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="spin_lidar_spin_joint_joint_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo reference="spin_lidar_spin_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="spin_lidar_spin_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <!-- ros_control plugin -->
    <plugin filename="libgazebo_ros_control_select_joints.so" name="gazebo_ros_control_select_joints">
      <robotNamespace>spin_lidar_control</robotNamespace>
      <joints>spin_lidar_spin_joint</joints>
    </plugin>
  </gazebo>
  <gazebo reference="spin_lidar_mount_link">
    <visual>
      <material>
        <ambient>0.1 0.1 0.1 1</ambient>
        <diffuse>0.48 0.48 0.48 1</diffuse>
        <emissive>0.237 0.237 0.237 1</emissive>
        <specular>0.8 0.8 0.8 1</specular>
      </material>
    </visual>
  </gazebo>
  <joint name="camera360_center_joint" type="fixed">
    <origin rpy="0.0 0 0" xyz="0 0 0.579"/>
    <parent link="autonomy_box_link"/>
    <child link="camera360_center_link"/>
  </joint>
  <link name="camera360_center_link">
    <!-- Sphere body -->
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.02"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.04"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
    <!-- Cable -->
    <!--      <visual>
        <origin xyz="${visual_radius + cable_collision_length/2} 0 0" rpy="0 0 0"/>
        <geometry>
          <box size="${cable_collision_length} 0.03 0.03"/>
        </geometry>
        <material name="Grey">
          <color rgba="0.5 0.5 0.5 1"/>
        </material> 
      </visual>-->
    <collision>
      <origin rpy="0 0 0" xyz="0.036000000000000004 0 0"/>
      <geometry>
        <box size="0.05 0.03 0.03"/>
      </geometry>
      <material name="Grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
  </link>
  <joint name="camera360_right_joint" type="fixed">
    <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 -0.01925392 0"/>
    <parent link="camera360_center_link"/>
    <child link="camera360_right_link"/>
  </joint>
  <link name="camera360_right_link"/>
  <joint name="camera360_right_optical_joint" type="fixed">
    <origin rpy="-1.5707963267948966 0.0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="camera360_right_link"/>
    <child link="camera360_right_optical_frame"/>
  </joint>
  <link name="camera360_right_optical_frame"/>
  <gazebo reference="camera360_right_link">
    <sensor name="camera360_right_camera_sensor" type="wideanglecamera">
      <update_rate>10</update_rate>
      <camera>
        <!--<horizontal_fov>${hfov * M_PI/180.0}</horizontal_fov>-->
        <horizontal_fov>3.141592653589793</horizontal_fov>
        <image>
          <format>R8G8B8</format>
          <width>1504</width>
          <height>1504</height>
        </image>
        <clip>
          <near>0.01</near>
          <far>100</far>
        </clip>
        <lens>
          <!--
            Crashes gazebo on some machines, so revert for now
            <type>custom</type>
            <custom_function>
              <c1>2</c1>
              <c2>2</c2>
              <f>1.0</f>
              <fun>sin</fun>
            </custom_function>
            <cutoff_angle>${fov/2}</cutoff_angle>
            <env_texture_size>1024</env_texture_size>
            -->
          <type>stereographic</type>
          <scale_to_hfov>true</scale_to_hfov>
          <cutoff_angle>1.5707</cutoff_angle>
          <env_texture_size>512</env_texture_size>
        </lens>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="camera360_right_camera_controller">
        <cameraName>camera360/right</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>not_used</cameraInfoTopicName>
        <frameName>camera360_right_optical_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <joint name="camera360_left_optical_joint" type="fixed">
    <origin rpy="-3.1402707 0.0036289 -3.1175768" xyz="0.002862424041614364  -0.00013127655875068252 -0.03724378451297336"/>
    <parent link="camera360_right_optical_frame"/>
    <child link="camera360_left_optical_frame"/>
  </joint>
  <link name="camera360_left_optical_frame"/>
  <joint name="camera360_left_joint" type="fixed">
    <origin rpy="0 -1.5707963267948966 1.5707963267948966" xyz="0 0 0"/>
    <parent link="camera360_left_optical_frame"/>
    <child link="camera360_left_link"/>
  </joint>
  <link name="camera360_left_link"/>
  <gazebo reference="camera360_left_link">
    <sensor name="camera360_left_camera_sensor" type="wideanglecamera">
      <update_rate>10</update_rate>
      <camera>
        <!--<horizontal_fov>${hfov * M_PI/180.0}</horizontal_fov>-->
        <horizontal_fov>3.141592653589793</horizontal_fov>
        <image>
          <format>R8G8B8</format>
          <width>1504</width>
          <height>1504</height>
        </image>
        <clip>
          <near>0.01</near>
          <far>100</far>
        </clip>
        <lens>
          <!--
            Crashes gazebo on some machines, so revert for now
            <type>custom</type>
            <custom_function>
              <c1>2</c1>
              <c2>2</c2>
              <f>1.0</f>
              <fun>sin</fun>
            </custom_function>
            <cutoff_angle>${fov/2}</cutoff_angle>
            <env_texture_size>1024</env_texture_size>
            -->
          <type>stereographic</type>
          <scale_to_hfov>true</scale_to_hfov>
          <cutoff_angle>1.5707</cutoff_angle>
          <env_texture_size>512</env_texture_size>
        </lens>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="camera360_left_camera_controller">
        <cameraName>camera360/left</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>not_used</cameraInfoTopicName>
        <frameName>camera360_left_optical_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <joint name="front_rgbd_cam_mount_joint" type="fixed">
    <origin rpy="0.0 0.4363323129985824 0" xyz="0.07187 -0.03 0.46295000000000003"/>
    <parent link="autonomy_box_link"/>
    <child link="front_rgbd_cam_mount_link"/>
  </joint>
  <link name="front_rgbd_cam_mount_link">
      </link>
  <joint name="front_rgbd_cam_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.010700000000000001 0.0175 0.0125"/>
    <parent link="front_rgbd_cam_mount_link"/>
    <child link="front_rgbd_cam_link"/>
  </joint>
  <link name="front_rgbd_cam_link">
    <inertial>
      <mass value="0.072"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4"/>
    </inertial>
    <visual>
      <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0.0042 -0.0175 0.0"/>
      <geometry>
        <mesh filename="package://hector_sensors_description/meshes/intel_realsense_d435/D435.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.008300000000000002 -0.0175 0.0"/>
      <geometry>
        <!--Dimensions taken from https://click.intel.com/intelr-realsensetm-depth-camera-d435.html-->
        <box size="0.025 0.09 0.025"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="front_rgbd_cam_link">
    <material>Gazebo/Grey</material>
  </gazebo>
  <gazebo reference="front_rgbd_cam_link">
    <sensor name="front_rgbd_cam" type="depth">
      <update_rate>20</update_rate>
      <camera>
        <horizontal_fov>1.2112585008840648</horizontal_fov>
        <image>
          <format>B8G8R8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.3</near>
          <far>9.0</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_openni_kinect.so" name="front_rgbd_cam_camera_controller">
        <cameraName>front_rgbd_cam</cameraName>
        <imageTopicName>color/image_rect_color</imageTopicName>
        <cameraInfoTopicName>color/camera_info</cameraInfoTopicName>
        <depthImageTopicName>depth/image_raw</depthImageTopicName>
        <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
        <pointCloudTopicName>depth/color/points</pointCloudTopicName>
        <frameName>front_rgbd_cam_color_optical_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <joint name="back_rgbd_cam_mount_joint" type="fixed">
    <origin rpy="0.0 0.4363323129985824 3.141592653589793" xyz="-0.07187 0.03 0.46295000000000003"/>
    <parent link="autonomy_box_link"/>
    <child link="back_rgbd_cam_mount_link"/>
  </joint>
  <link name="back_rgbd_cam_mount_link">
      </link>
  <joint name="back_rgbd_cam_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.010700000000000001 0.0175 0.0125"/>
    <parent link="back_rgbd_cam_mount_link"/>
    <child link="back_rgbd_cam_link"/>
  </joint>
  <link name="back_rgbd_cam_link">
    <inertial>
      <mass value="0.072"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4"/>
    </inertial>
    <visual>
      <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0.0042 -0.0175 0.0"/>
      <geometry>
        <mesh filename="package://hector_sensors_description/meshes/intel_realsense_d435/D435.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.008300000000000002 -0.0175 0.0"/>
      <geometry>
        <!--Dimensions taken from https://click.intel.com/intelr-realsensetm-depth-camera-d435.html-->
        <box size="0.025 0.09 0.025"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="back_rgbd_cam_link">
    <material>Gazebo/Grey</material>
  </gazebo>
  <gazebo reference="back_rgbd_cam_link">
    <sensor name="back_rgbd_cam" type="depth">
      <update_rate>20</update_rate>
      <camera>
        <horizontal_fov>1.2112585008840648</horizontal_fov>
        <image>
          <format>B8G8R8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.3</near>
          <far>9.0</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_openni_kinect.so" name="back_rgbd_cam_camera_controller">
        <cameraName>back_rgbd_cam</cameraName>
        <imageTopicName>color/image_rect_color</imageTopicName>
        <cameraInfoTopicName>color/camera_info</cameraInfoTopicName>
        <depthImageTopicName>depth/image_raw</depthImageTopicName>
        <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
        <pointCloudTopicName>depth/color/points</pointCloudTopicName>
        <frameName>back_rgbd_cam_color_optical_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <joint name="tracker_joint" type="fixed">
    <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="-0.07 -0.06 0.464  "/>
    <parent link="autonomy_box_link"/>
    <child link="tracker_link"/>
  </joint>
  <link name="tracker_link">
    <inertial>
      <mass value="0.089"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
    <!-- inertia not measured yet -->
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hector_sensors_description/meshes/vive_tracker/tracker.stl"/>
      </geometry>
      <material name="DarkGrey">
        <color rgba="0.175 0.175 0.175 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.021144 -0.003673"/>
      <geometry>
        <box size="0.0944 0.0423 0.0921"/>
        <!-- Bounding box extracted from STL -->
      </geometry>
    </collision>
  </link>
  <gazebo reference="tracker_link">
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_p3d.so" name="tracker_odom">
      <frameName>world</frameName>
      <bodyName>base_link</bodyName>
      <topicName>/tracker/odometry</topicName>
      <updateRate>100.0</updateRate>
      <!--gaussianNoise>0.0001</gaussianNoise-->
    </plugin>
  </gazebo>
  <gazebo reference="autonomy_box_link">
    <visual>
      <material>
        <ambient>0.1 0.1 0.1 1</ambient>
        <diffuse>0.48 0.48 0.48 1</diffuse>
        <emissive>0.237 0.237 0.237 1</emissive>
        <specular>0.8 0.8 0.8 1</specular>
      </material>
    </visual>
  </gazebo>
  <joint name="sensor_head_mount_joint" type="fixed">
    <origin rpy="0.0 0.0 3.141592653589793" xyz="-0.2815 0.00856 0.093"/>
    <parent link="base_link"/>
    <child link="sensor_head_mount_link"/>
  </joint>
  <link name="sensor_head_mount_link">
    <inertial>
      <mass value="0.09"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="2.25e-05" ixy="0.0" ixz="0.0" iyy="2.25e-05" iyz="0.0" izz="2.25e-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hector_components_description/meshes/xm430_sensor_head/sensor_head_link_0.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 -0.01 0.017"/>
      <geometry>
        <box size="0.0285 0.07 0.034"/>
      </geometry>
    </collision>
  </link>
  <joint name="sensor_head_yaw_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
    <parent link="sensor_head_mount_link"/>
    <child link="sensor_head_yaw_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="4" lower="-1.5707963267948966" upper="1.5707963267948966" velocity="4.81710873"/>
  </joint>
  <link name="sensor_head_yaw_link">
    <inertial>
      <mass value="0.09"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="4.41e-05" ixy="0.0" ixz="0.0" iyy="4.41e-05" iyz="0.0" izz="4.41e-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.036"/>
      <geometry>
        <mesh filename="package://hector_components_description/meshes/xm430_sensor_head/sensor_head_link_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0625"/>
      <geometry>
        <box size="0.0285 0.034 0.055"/>
      </geometry>
    </collision>
  </link>
  <joint name="sensor_head_pitch_joint" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="0.0 0.0 0.0765"/>
    <parent link="sensor_head_yaw_link"/>
    <child link="sensor_head_pitch_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="4" lower="-1.5707963267948966" upper="1.5707963267948966" velocity="4.81710873"/>
  </joint>
  <link name="sensor_head_pitch_link">
    <inertial>
      <mass value="0.03"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="1.4700000000000003e-05" ixy="0.0" ixz="0.0" iyy="1.4700000000000003e-05" iyz="0.0" izz="1.4700000000000003e-05"/>
    </inertial>
    <visual>
      <origin rpy="1.5707963267948966 3.141592653589793 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hector_components_description/meshes/xm430_sensor_head/sensor_head_link_2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 -0.0255 0.0"/>
      <geometry>
        <box size="0.025 0.005 0.045"/>
      </geometry>
      <material name="DarkGrey"/>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 -0.0075 0.02"/>
      <geometry>
        <box size="0.025 0.035 0.006"/>
      </geometry>
      <material name="DarkGrey"/>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 -0.0075 -0.02"/>
      <geometry>
        <box size="0.025 0.035 0.006"/>
      </geometry>
      <material name="DarkGrey"/>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 -0.0075 0.0"/>
      <geometry>
        <box size="0.025 0.035 0.045"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="sensor_head_yaw_link">
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="sensor_head_pitch_link">
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="sensor_head_mount_link">
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <transmission name="sensor_head_yaw_joint_joint_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="sensor_head_yaw_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="sensor_head_yaw_joint_joint_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo reference="sensor_head_yaw_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <transmission name="sensor_head_pitch_joint_joint_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="sensor_head_pitch_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="sensor_head_pitch_joint_joint_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo reference="sensor_head_pitch_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_control_select_joints.so" name="gazebo_ros_control_select_joints">
      <robotNamespace>sensor_head_control</robotNamespace>
      <joints>sensor_head_yaw_joint sensor_head_pitch_joint</joints>
    </plugin>
  </gazebo>
  <joint name="sensor_head_thermal_cam_joint" type="fixed">
    <origin rpy="1.5707963267948966 0 0" xyz="-0.01 -0.0692 0"/>
    <parent link="sensor_head_pitch_link"/>
    <child link="sensor_head_thermal_cam_frame"/>
  </joint>
  <link name="sensor_head_thermal_cam_frame">
    <inertial>
      <mass value="0.03"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="1.2000000000000002e-06" ixy="0.0" ixz="0.0" iyy="1.2000000000000002e-06" iyz="0.0" izz="1.2000000000000002e-06"/>
    </inertial>
  </link>
  <joint name="sensor_head_thermal_cam_optical_joint" type="fixed">
    <origin rpy="-1.5707963267948966 0.0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="sensor_head_thermal_cam_frame"/>
    <child link="sensor_head_thermal_cam_optical_frame"/>
  </joint>
  <link name="sensor_head_thermal_cam_optical_frame"/>
  <gazebo reference="sensor_head_thermal_cam_frame">
    <material>Gazebo/Grey</material>
  </gazebo>
  <gazebo reference="sensor_head_thermal_cam_frame">
    <sensor name="thermal_camera_sensor" type="camera">
      <update_rate>10</update_rate>
      <camera>
        <horizontal_fov>1.5707963267948966</horizontal_fov>
        <image>
          <format>R8G8B8</format>
          <width>206</width>
          <height>156</height>
        </image>
        <clip>
          <near>0.01</near>
          <far>10</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_thermal_camera.so" name="thermal_camera_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>10</updateRate>
        <cameraName>sensor_head_thermal_cam</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>sensor_head_thermal_cam_optical_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <joint name="sensor_head_rgbd_cam_joint" type="fixed">
    <origin rpy="1.5707963267948966 0.0 0.0" xyz="0.0253 -0.043 0.0175"/>
    <parent link="sensor_head_pitch_link"/>
    <child link="sensor_head_rgbd_cam_link"/>
  </joint>
  <link name="sensor_head_rgbd_cam_link">
    <inertial>
      <mass value="0.072"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4"/>
    </inertial>
    <visual>
      <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0.0042 -0.0175 0.0"/>
      <geometry>
        <mesh filename="package://hector_sensors_description/meshes/intel_realsense_d435/D435.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.008300000000000002 -0.0175 0.0"/>
      <geometry>
        <!--Dimensions taken from https://click.intel.com/intelr-realsensetm-depth-camera-d435.html-->
        <box size="0.025 0.09 0.025"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="sensor_head_rgbd_cam_link">
    <material>Gazebo/Grey</material>
  </gazebo>
  <gazebo reference="sensor_head_rgbd_cam_link">
    <sensor name="sensor_head_rgbd_cam" type="depth">
      <update_rate>20</update_rate>
      <camera>
        <horizontal_fov>1.2112585008840648</horizontal_fov>
        <image>
          <format>B8G8R8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.3</near>
          <far>9.0</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_openni_kinect.so" name="sensor_head_rgbd_cam_camera_controller">
        <cameraName>sensor_head_rgbd_cam</cameraName>
        <imageTopicName>color/image_rect_color</imageTopicName>
        <cameraInfoTopicName>color/camera_info</cameraInfoTopicName>
        <depthImageTopicName>depth/image_raw</depthImageTopicName>
        <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
        <pointCloudTopicName>depth/points</pointCloudTopicName>
        <frameName>sensor_head_rgbd_cam_color_optical_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <!-- Gazebo ros control for arm joints -->
  <!-- Moved outside -->
  <!--    <gazebo>
      <plugin name="gazebo_ros_control_select_joints" filename="libgazebo_ros_control_select_joints.so">
        <robotNamespace>manipulator_arm_control</robotNamespace>
        <joints>${name}_joint_0 ${name}_joint_1 ${name}_joint_2 ${name}_joint_3 ${name}_joint_4 ${name}_joint_5</joints>
        <eStopTopic>/ui_emergency_stop</eStopTopic>
      </plugin>
    </gazebo>-->
  <!-- Link 0 -->
  <gazebo reference="arm_link_0">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Link 1 -->
  <gazebo reference="arm_link_1">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Link 2 -->
  <gazebo reference="arm_link_2">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Link 3 -->
  <gazebo reference="arm_link_3">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Link 4 -->
  <gazebo reference="arm_link_4">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Link 5 -->
  <gazebo reference="arm_link_5">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- End Effector -->
  <gazebo reference="arm_end_link">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <joint name="arm_fixed_joint" type="fixed">
    <origin rpy="0.0 0.0 3.141592653589793" xyz="-0.2115 -0.093 0.061"/>
    <parent link="base_link"/>
    <child link="arm_link_0"/>
  </joint>
  <!-- link 0 -->
  <link name="arm_link_0">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.063"/>
      <geometry>
        <box size="0.054 0.054 0.126"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://open_manipulator_p_description/meshes/link_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.00023 0.00000 0.06223"/>
      <mass value="0.85644"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      <!--<inertia ixx="0.00121064" ixy="0.0" ixz="0.00001100" iyy="0.00120886" iyz="0.0" izz="0.00038103" />-->
    </inertial>
  </link>
  <!-- Joint 0 -->
  <joint name="arm_joint_0" type="revolute">
    <parent link="arm_link_0"/>
    <child link="arm_link_1"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 0.126"/>
    <axis xyz="0 0 1"/>
    <limit effort="1000" lower="-3.106686068549907" upper="3.106686068549907" velocity="6.5"/>
    <dynamics damping="0.7"/>
  </joint>
  <transmission name="tran0">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="arm_joint_0">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor0">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- link 1 -->
  <link name="arm_link_1">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://open_manipulator_p_description/meshes/link_2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.00000 0.00080 0.03183"/>
      <!--<origin xyz="0.00000 -0.06820 -0.00117" rpy="0 0 0"/>-->
      <mass value="0.94658"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      <!--<inertia ixx="0.00152547" ixy="0.0" ixz="0.0" iyy="0.00045954" iyz="-0.00001086" izz="0.00151740" />-->
    </inertial>
  </link>
  <!-- Joint 1 -->
  <joint name="arm_joint_1" type="revolute">
    <parent link="arm_link_1"/>
    <child link="arm_link_2"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 0.033"/>
    <axis xyz="0 1 0"/>
    <limit effort="1000" lower="-1.8151424220741028" upper="1.8151424220741028" velocity="6.5"/>
    <dynamics damping="0.7"/>
  </joint>
  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="arm_joint_1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- link 2 -->
  <link name="arm_link_2">
    <collision>
      <origin rpy="0 0 0" xyz="0.0 0.07 0.01"/>
      <geometry>
        <box size="0.06 0.02 0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 -0.07 0.01"/>
      <geometry>
        <box size="0.06 0.02 0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.045"/>
      <geometry>
        <box size="0.06 0.16 0.02"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.155"/>
      <geometry>
        <box size="0.06 0.09 0.2"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0.7853981633974483 0" xyz="0.025 0.0 0.26"/>
      <geometry>
        <box size="0.07 0.115 0.08"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0.069 0"/>
      <geometry>
        <mesh filename="package://open_manipulator_p_description/meshes/link_3.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01768 0.00019 0.20524"/>
      <mass value="1.30260"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      <!--<inertia ixx="0.01111021" ixy="0.00001331" ixz="-0.00134208" iyy="0.01046642" iyz="0.00016235" izz="0.00202081" />-->
    </inertial>
  </link>
  <!-- Joint 2 -->
  <joint name="arm_joint_2" type="revolute">
    <parent link="arm_link_2"/>
    <child link="arm_link_3"/>
    <origin rpy="0 0 0" xyz="0.03000 0.0 0.26400"/>
    <axis xyz="0 1 0"/>
    <limit effort="1000" lower="-2.1642082724729685" upper="1.4835298641951802" velocity="6.5"/>
    <dynamics damping="0.7"/>
  </joint>
  <transmission name="tran2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="arm_joint_2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- link 3 -->
  <link name="arm_link_3">
    <collision>
      <origin rpy="0 0.7853981633974483 0" xyz="0.01 0.06 0.01"/>
      <geometry>
        <box size="0.06 0.02 0.1"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0.7853981633974483 0" xyz="0.01 -0.06 0.01"/>
      <geometry>
        <box size="0.06 0.02 0.1"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.13 0 0.03"/>
      <geometry>
        <box size="0.2 0.09 0.06"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.04 0 0.03"/>
      <geometry>
        <box size="0.03 0.14 0.06"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0.05750 0"/>
      <geometry>
        <mesh filename="package://open_manipulator_p_description/meshes/link_4.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.07025 0.00034 -0.00021"/>
      <mass value="1.15977"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      <!--<inertia ixx="0.00096726" ixy="0.00004383" ixz="-0.00002771" iyy="0.00318801" iyz="0.00000627" izz="0.00341484" />-->
    </inertial>
  </link>
  <!-- Joint 3 -->
  <joint name="arm_joint_3" type="revolute">
    <parent link="arm_link_3"/>
    <child link="arm_link_4"/>
    <origin rpy="0 0 0" xyz="0.19500 0.0 0.03000"/>
    <axis xyz="1 0 0"/>
    <limit effort="1000" lower="-3.106686068549907" upper="3.106686068549907" velocity="6.5"/>
    <dynamics damping="0.7"/>
  </joint>
  <transmission name="tran3">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="arm_joint_3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- link 4 -->
  <link name="arm_link_4">
    <collision>
      <origin rpy="0 0 0" xyz="0.06 0 0.0"/>
      <geometry>
        <box size="0.055 0.09 0.05"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://open_manipulator_p_description/meshes/link_5.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.05642 -0.00217 0.00002"/>
      <!--<origin xyz="-0.00658 -0.04717 0.00002" rpy="0 0 0"/>-->
      <mass value="0.44688"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      <!--<inertia ixx="0.00030285" ixy="0.00000761" ixz="0.00000031" iyy="0.00022339" iyz="-0.00000001" izz="0.00039766" />-->
    </inertial>
  </link>
  <!-- Joint 4 -->
  <joint name="arm_joint_4" type="revolute">
    <parent link="arm_link_4"/>
    <child link="arm_link_5"/>
    <origin rpy="0 0 0" xyz="0.06300 0.00 0.00000"/>
    <axis xyz="0 1 0"/>
    <limit effort="1000" lower="-1.6580627893946132" upper="1.6580627893946132" velocity="6.5"/>
    <dynamics damping="0.7"/>
  </joint>
  <transmission name="tran4">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="arm_joint_4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- link 5 -->
  <link name="arm_link_5">
    <collision>
      <origin rpy="0 0 0" xyz="0.005 0.045 0.0"/>
      <geometry>
        <box size="0.07 0.015 0.05"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.005 -0.045 0.0"/>
      <geometry>
        <box size="0.07 0.015 0.05"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.035 0.0 0.0"/>
      <geometry>
        <box size="0.02 0.105 0.05"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.08 0.0 0.0"/>
      <geometry>
        <box size="0.09 0.045 0.045"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0.04500 0"/>
      <geometry>
        <mesh filename="package://open_manipulator_p_description/meshes/link_6.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <inertial>
      <!-- <origin xyz="0.06831 -0.04457 0.00000" rpy="0 0 0"/> -->
      <origin rpy="0 0 0" xyz="-0.05469 0.00043 0.00000"/>
      <mass value="0.43273"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      <!--<inertia ixx="0.00021512" ixy="0.00001009" ixz="0.00000000" iyy="0.00042827" iyz="0.00000000" izz="0.00051436" />-->
    </inertial>
  </link>
  <!-- Joint 5 -->
  <joint name="arm_joint_5" type="revolute">
    <parent link="arm_link_5"/>
    <child link="arm_end_link"/>
    <origin rpy="0 0 0" xyz="0.12300 0.000 0.00000"/>
    <axis xyz="1 0 0"/>
    <limit effort="1000" lower="-3.07177948351002" upper="3.07177948351002" velocity="6.5"/>
    <dynamics damping="0.7"/>
  </joint>
  <transmission name="tran5">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="arm_joint_5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- end link -->
  <link name="arm_end_link">
    <collision>
      <origin rpy="0 0 0" xyz="0.01 0.0 0.0"/>
      <geometry>
        <box size="0.02 0.06 0.045"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://open_manipulator_p_description/meshes/end_link.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.00337 0.00000 0.00000"/>
      <mass value="0.01919"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      <!--<inertia ixx="0.00000938" ixy="0.0" ixz="0.0" iyy="0.00000338" iyz="0.0" izz="0.00000635" />-->
    </inertial>
  </link>
  <!-- Base Joint -->
  <joint name="gripper_base_joint" type="fixed">
    <origin rpy="0 1.5707963267948966 0" xyz="0.0 0 0"/>
    <parent link="arm_end_link"/>
    <child link="gripper_base"/>
  </joint>
  <!-- Base -->
  <link name="gripper_base">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rh_p12_rn_description/meshes/base_convex.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rh_p12_rn_description/meshes/base.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <!--
      <inertial>
        <origin xyz="0.000 0.000 0.032" rpy="0 0 0"/>
        <mass value="0.236"/>-->
    <!--<inertia ixx="0.00017" ixy="0.0" ixz="0.0" iyy="0.00011" iyz="0.0" izz="0.00017" />-->
    <!-- <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
      </inertial>
      -->
    <inertial>
      <origin rpy="0 0 0" xyz="0.000 0.000 0.032"/>
      <mass value="0.236"/>
      <inertia ixx="0.00025173333333333334" ixy="0.0" ixz="0.0" iyy="0.00025173333333333334" iyz="0.0" izz="0.00025173333333333334"/>
    </inertial>
  </link>
  <gazebo reference="gripper_base">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Finger Motor Joint -->
  <joint name="gripper_servo_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0 0.0 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="4" lower="0" upper="1.15" velocity="5"/>
    <parent link="gripper_base"/>
    <child link="gripper_servo_link"/>
  </joint>
  <transmission name="gripper_servo_joint_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="gripper_servo_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="gripper_servo_joint_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="gripper_servo_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <mass value="0.01"/>
      <inertia ixx="2.666666666666667e-06" ixy="0.0" ixz="0.0" iyy="2.666666666666667e-06" iyz="0.0" izz="2.666666666666667e-06"/>
    </inertial>
  </link>
  <!-- Right Joint 1-->
  <joint name="gripper_joint_r1" type="revolute">
    <parent link="gripper_base"/>
    <child link="gripper_link_r1"/>
    <origin rpy="0 0 0" xyz="0.0 0.008 0.048"/>
    <axis xyz="1 0 0"/>
    <limit effort="10" lower="0.0" upper="1.1" velocity="6.5"/>
    <!--<dynamics damping="0.7"/>-->
    <!--WARNING: This makes Gazebo sim explode-->
    <mimic joint="gripper_servo_joint" multiplier="1" offset="0"/>
  </joint>
  <gazebo>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_gripper_joint_r1">
      <joint>gripper_servo_joint</joint>
      <mimicJoint>gripper_joint_r1</mimicJoint>
      <multiplier>1</multiplier>
      <offset>0</offset>
      <maxEffort>4</maxEffort>
      <hasPID/>
    </plugin>
  </gazebo>
  <!-- Right Link 1 -->
  <link name="gripper_link_r1">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rh_p12_rn_description/meshes/r1_convex.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rh_p12_rn_description/meshes/r1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <!--
      <inertial>
        <origin xyz="0.000 0.034 0.004" rpy="0 0 0"/>
        <mass value="0.068"/>-->
    <!--<inertia ixx="0.00004" ixy="0.0" ixz="0.0" iyy="0.00002" iyz="-0.00001" izz="0.00002" />-->
    <!--<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
      </inertial>
      -->
    <inertial>
      <origin rpy="0 0 0" xyz="0.000 0.034 0.004"/>
      <mass value="0.068"/>
      <inertia ixx="1.8133333333333335e-05" ixy="0.0" ixz="0.0" iyy="1.8133333333333335e-05" iyz="0.0" izz="1.8133333333333335e-05"/>
    </inertial>
  </link>
  <gazebo reference="gripper_link_r1">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Right Joint 2 -->
  <joint name="gripper_joint_r2" type="revolute">
    <parent link="gripper_link_r1"/>
    <child link="gripper_link_r2"/>
    <origin rpy="0 0 3.141592653589793" xyz="0.0 0.0493634 0.0285"/>
    <axis xyz="1 0 0"/>
    <limit effort="10" lower="0.0" upper="1.0" velocity="6.5"/>
    <!--<dynamics damping="0.7"/>-->
    <!--WARNING: This makes Gazebo sim explode-->
    <mimic joint="gripper_servo_joint" multiplier="0.909" offset="0"/>
    s
    
  </joint>
  <gazebo>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_gripper_joint_r2">
      <joint>gripper_servo_joint</joint>
      <mimicJoint>gripper_joint_r2</mimicJoint>
      <multiplier>0.909</multiplier>
      <offset>0</offset>
      <maxEffort>4</maxEffort>
      <hasPID/>
    </plugin>
  </gazebo>
  <!-- Right Link 2 -->
  <link name="gripper_link_r2">
    <collision>
      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rh_p12_rn_description/meshes/r2_convex.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rh_p12_rn_description/meshes/r2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <!--
      <inertial>
        <origin xyz="0.000 0.006 0.011" rpy="0 0 0"/>
        <mass value="0.022"/>-->
    <!--<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.0" />-->
    <!--<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
      </inertial>-->
    <inertial>
      <origin rpy="0 0 0" xyz="0.000 -0.006 0.011"/>
      <mass value="0.022"/>
      <inertia ixx="5.866666666666666e-06" ixy="0.0" ixz="0.0" iyy="5.866666666666666e-06" iyz="0.0" izz="5.866666666666666e-06"/>
    </inertial>
  </link>
  <gazebo reference="gripper_link_r2">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Left Joint 1 -->
  <joint name="gripper_joint_l1" type="revolute">
    <parent link="gripper_base"/>
    <child link="gripper_link_l1"/>
    <origin rpy="0 0 3.141592653589793" xyz="0.0 -0.008 0.048"/>
    <axis xyz="1 0 0"/>
    <limit effort="10" lower="0.0" upper="1.1" velocity="6.5"/>
    <!--<dynamics damping="0.7"/>-->
    <!--WARNING: This makes Gazebo sim explode-->
    <mimic joint="gripper_servo_joint" multiplier="1" offset="0"/>
  </joint>
  <gazebo>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_gripper_joint_l1">
      <joint>gripper_servo_joint</joint>
      <mimicJoint>gripper_joint_l1</mimicJoint>
      <multiplier>1</multiplier>
      <offset>0</offset>
      <maxEffort>4</maxEffort>
      <hasPID/>
    </plugin>
  </gazebo>
  <!-- Left Link 1 -->
  <link name="gripper_link_l1">
    <collision>
      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rh_p12_rn_description/meshes/l1_convex.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rh_p12_rn_description/meshes/l1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <!--
      <inertial>
        <origin xyz="0.000 -0.034 0.004" rpy="0 0 0"/>
        <mass value="0.068"/>-->
    <!--<inertia ixx="0.00004" ixy="0.0" ixz="0.0" iyy="0.00002" iyz="0.00001" izz="0.00002" />-->
    <!--<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
      </inertial>-->
    <inertial>
      <origin rpy="0 0 0" xyz="0.000 0.034 0.004"/>
      <mass value="0.068"/>
      <inertia ixx="1.8133333333333335e-05" ixy="0.0" ixz="0.0" iyy="1.8133333333333335e-05" iyz="0.0" izz="1.8133333333333335e-05"/>
    </inertial>
  </link>
  <gazebo reference="gripper_link_l1">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Left Joint 2 -->
  <joint name="gripper_joint_l2" type="revolute">
    <parent link="gripper_link_l1"/>
    <child link="gripper_link_l2"/>
    <origin rpy="0 0 3.141592653589793" xyz="0.0 0.0493634 0.0285"/>
    <axis xyz="1 0 0"/>
    <limit effort="10" lower="0.0" upper="1.0" velocity="6.5"/>
    <!--<dynamics damping="0.7"/>-->
    <!--WARNING: This makes Gazebo sim explode-->
    s
      
    <mimic joint="gripper_servo_joint" multiplier="0.909" offset="0"/>
  </joint>
  <gazebo>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_gripper_joint_l2">
      <joint>gripper_servo_joint</joint>
      <mimicJoint>gripper_joint_l2</mimicJoint>
      <multiplier>0.909</multiplier>
      <offset>0</offset>
      <maxEffort>4</maxEffort>
      <hasPID/>
    </plugin>
  </gazebo>
  <!-- Left Link 2 -->
  <link name="gripper_link_l2">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rh_p12_rn_description/meshes/l2_convex.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rh_p12_rn_description/meshes/l2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <!--
      <inertial>
        <origin xyz="0.000 -0.006 0.011" rpy="0 0 0"/>
        <mass value="0.022"/>-->
    <!--<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.0" />-->
    <!--<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
      </inertial>-->
    <inertial>
      <origin rpy="0 0 0" xyz="0.000 -0.006 0.011"/>
      <mass value="0.022"/>
      <inertia ixx="5.866666666666666e-06" ixy="0.0" ixz="0.0" iyy="5.866666666666666e-06" iyz="0.0" izz="5.866666666666666e-06"/>
    </inertial>
  </link>
  <joint name="gripper_grasp_main_joint" type="fixed">
    <origin rpy="3.141592653589793 -1.5707963267948966 0.0" xyz="0.0 0 0.11"/>
    <parent link="gripper_base"/>
    <child link="gripper_grasp_main_link"/>
  </joint>
  <link name="gripper_grasp_main_link">
    </link>
  <joint name="gripper_grasp_tip_joint" type="fixed">
    <origin rpy="3.141592653589793 -1.5707963267948966 0.0" xyz="0.0 0 0.15"/>
    <parent link="gripper_base"/>
    <child link="gripper_grasp_tip_link"/>
  </joint>
  <link name="gripper_grasp_tip_link">
    </link>
  <gazebo reference="gripper_link_l2">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_control_select_joints.so" name="gazebo_ros_control_select_joints">
      <robotNamespace>manipulator_arm_control</robotNamespace>
      <joints>arm_joint_0 arm_joint_1 arm_joint_2 arm_joint_3 arm_joint_4 arm_joint_5 gripper_servo_joint</joints>
      <eStopTopic>/ui_emergency_stop</eStopTopic>
    </plugin>
  </gazebo>
</robot>