<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from dual_arm_husky.urdf.xacro      | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from description.xacro              | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!--
Software License Agreement (BSD)

\file      description.urdf.xacro
\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>, Devon Ash <dash@clearpathrobotics.com>
\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that
the following conditions are met:
 * Redistributions of source code must retain the above copyright notice, this list of conditions and the
   following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
   following disclaimer in the documentation and/or other materials provided with the distribution.
 * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
   products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<robot name="husky" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <mujoco>
        <compiler 
        meshdir="../meshes_mujoco/" 
        fusestatic= "true" 
        balanceinertia="true" 
        strippath="true" 
        discardvisual="false" />
  </mujoco>
  <!-- Included URDF/XACRO Files -->
  <!-- Inertia parameters -->
  <!-- This mass might be incorrect -->
  <!-- Kinematic model -->
  <!-- Properties from urcontrol.conf -->
  <!--
    DH for UR5:
    a = [0.00000, -0.42500, -0.39225,  0.00000,  0.00000,  0.0000]
    d = [0.089159,  0.00000,  0.00000,  0.10915,  0.09465,  0.0823]
    alpha = [ 1.570796327, 0, 0, 1.570796327, -1.570796327, 0 ]
    q_home_offset = [0, -1.570796327, 0, -1.570796327, 0, 0]
    joint_direction = [-1, -1, 1, 1, 1, 1]
    mass = [3.7000, 8.3930, 2.2750, 1.2190, 1.2190, 0.1879]
    center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0, 0.0265], [0, -0.0018, 0.01634], [0, 0.0018,0.01634], [0, 0, -0.001159] ]
  -->
  <!-- Arbitrary offsets for shoulder/elbow joints -->
  <!-- measured from model -->
  <!-- measured from model -->
  <!-- link lengths used in model -->
  <!--property name="shoulder_height" value="0.089159" /-->
  <!--property name="shoulder_offset" value="0.13585" /-->
  <!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 -->
  <!--property name="upper_arm_length" value="0.42500" /-->
  <!--property name="elbow_offset" value="0.1197" /-->
  <!-- CAD measured -->
  <!--property name="forearm_length" value="0.39225" /-->
  <!--property name="wrist_1_length" value="0.093" /-->
  <!-- CAD measured -->
  <!--property name="wrist_2_length" value="0.09465" /-->
  <!-- In CAD this distance is 0.930, but in the spec it is 0.09465 -->
  <!--property name="wrist_3_length" value="0.0823" /-->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- manually measured -->
  <!-- Base Size -->
  <!-- Wheel Mounting Positions -->
  <!-- Wheel Properties -->
  <!-- Base link is the center of the robot's bottom plate -->
  <link name="base_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://husky_description/meshes/base_link.stl"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.061875"/>
      <geometry>
        <box size="0.9874 0.5709 0.12375"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.185625"/>
      <geometry>
        <box size="0.78992 0.5709 0.12375"/>
      </geometry>
    </collision>
  </link>
  <!-- Base footprint is on the ground under the robot -->
  <link name="base_footprint"/>
  <joint name="base_footprint_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 -0.14493"/>
    <parent link="base_link"/>
    <child link="base_footprint"/>
  </joint>
  <!-- Interial link stores the robot's inertial information -->
  <link name="inertial_link">
    <inertial>
      <mass value="0.39054"/>
      <origin xyz="-0.0648 -0.00059 0.07895"/>
      <inertia ixx="0.001634275613800003" ixy="-0.0055452352110000006" ixz="-0.041083822026" iyy="0.05314689495400002" iyz="-0.0004366145530000001" izz="-0.004759585875"/>
    </inertial>
  </link>
  <joint name="inertial_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="base_link"/>
    <child link="inertial_link"/>
  </joint>
  <!-- IMU Link is the standard mounting position for the UM6 IMU.-->
  <!-- Can be modified with environment variables in /etc/ros/setup.bash -->
  <link name="imu_link"/>
  <joint name="imu_joint" type="fixed">
    <!-- <origin xyz="$(optenv HUSKY_IMU_XYZ 0.19 0 0.149)" rpy="$(optenv HUSKY_IMU_RPY 0 -1.5708 3.1416)" /> -->
    <origin rpy="-1.5708 0 0" xyz="0.19 0 0.149"/>
    <parent link="base_link"/>
    <child link="imu_link"/>
  </joint>
  <gazebo reference="imu_link">
    </gazebo>
  <!-- Husky wheel macros -->
  <link name="front_left_wheel_link">
    <inertial>
      <mass value="2.637"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://husky_description/meshes/wheel.stl"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.1143" radius="0.17775"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="front_left_wheel_link">
    <mu1 value="1.0"/>
    <mu2 value="1.0"/>
    <kp value="10000000.0"/>
    <kd value="1.0"/>
    <fdir1 value="1 0 0"/>
  </gazebo>
  <joint name="front_left_wheel" type="continuous">
    <parent link="base_link"/>
    <child link="front_left_wheel_link"/>
    <origin rpy="0 0 0" xyz="0.256 0.2854 0.03282"/>
    <axis rpy="0 0 0" xyz="0 1 0"/>
  </joint>
  <transmission name="front_left_wheel_trans" type="SimpleTransmission">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="front_left_wheel_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="front_left_wheel">
      <hardwareInterface>VelocityJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <link name="front_right_wheel_link">
    <inertial>
      <mass value="2.637"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://husky_description/meshes/wheel.stl"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.1143" radius="0.17775"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="front_right_wheel_link">
    <mu1 value="1.0"/>
    <mu2 value="1.0"/>
    <kp value="10000000.0"/>
    <kd value="1.0"/>
    <fdir1 value="1 0 0"/>
  </gazebo>
  <joint name="front_right_wheel" type="continuous">
    <parent link="base_link"/>
    <child link="front_right_wheel_link"/>
    <origin rpy="0 0 0" xyz="0.256 -0.2854 0.03282"/>
    <axis rpy="0 0 0" xyz="0 1 0"/>
  </joint>
  <transmission name="front_right_wheel_trans" type="SimpleTransmission">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="front_right_wheel_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="front_right_wheel">
      <hardwareInterface>VelocityJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <link name="rear_left_wheel_link">
    <inertial>
      <mass value="2.637"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://husky_description/meshes/wheel.stl"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.1143" radius="0.17775"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="rear_left_wheel_link">
    <mu1 value="1.0"/>
    <mu2 value="1.0"/>
    <kp value="10000000.0"/>
    <kd value="1.0"/>
    <fdir1 value="1 0 0"/>
  </gazebo>
  <joint name="rear_left_wheel" type="continuous">
    <parent link="base_link"/>
    <child link="rear_left_wheel_link"/>
    <origin rpy="0 0 0" xyz="-0.256 0.2854 0.03282"/>
    <axis rpy="0 0 0" xyz="0 1 0"/>
  </joint>
  <transmission name="rear_left_wheel_trans" type="SimpleTransmission">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="rear_left_wheel_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="rear_left_wheel">
      <hardwareInterface>VelocityJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <link name="rear_right_wheel_link">
    <inertial>
      <mass value="2.637"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://husky_description/meshes/wheel.stl"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.1143" radius="0.17775"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="rear_right_wheel_link">
    <mu1 value="1.0"/>
    <mu2 value="1.0"/>
    <kp value="10000000.0"/>
    <kd value="1.0"/>
    <fdir1 value="1 0 0"/>
  </gazebo>
  <joint name="rear_right_wheel" type="continuous">
    <parent link="base_link"/>
    <child link="rear_right_wheel_link"/>
    <origin rpy="0 0 0" xyz="-0.256 -0.2854 0.03282"/>
    <axis rpy="0 0 0" xyz="0 1 0"/>
  </joint>
  <transmission name="rear_right_wheel_trans" type="SimpleTransmission">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="rear_right_wheel_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="rear_right_wheel">
      <hardwareInterface>VelocityJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <!-- Spawn Husky chassis -->
  <link name="top_chassis_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://husky_description/meshes/top_chassis.stl"/>
      </geometry>
    </visual>
  </link>
  <!-- Attach chassis to the robot -->
  <joint name="top_chassis_joint" type="fixed">
    <parent link="base_link"/>
    <child link="top_chassis_link"/>
  </joint>
  <!-- Spawn front bumper link -->
  <link name="front_bumper_link">
    <visual>
      <geometry>
        <mesh filename="package://husky_description/meshes/bumper.stl"/>
      </geometry>
    </visual>
  </link>
  <!-- Attach front bumper -->
  <joint name="front_bumper" type="fixed">
    <origin rpy="0 0 0" xyz="0.48 0 0.091"/>
    <parent link="base_link"/>
    <child link="front_bumper_link"/>
  </joint>
  <!-- Spawn rear bumper link -->
  <link name="rear_bumper_link">
    <visual>
      <geometry>
        <mesh filename="package://husky_description/meshes/bumper.stl"/>
      </geometry>
    </visual>
  </link>
  <!-- Attach rear bumper -->
  <joint name="rear_bumper" type="fixed">
    <origin rpy="0 0 3.14159" xyz="-0.48 0 0.091"/>
    <parent link="base_link"/>
    <child link="rear_bumper_link"/>
  </joint>
  <!-- Spawn the bulkhead -->
  <link name="dual_arm_bulkhead_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://husky_description/meshes/dual_arm_bulkhead.stl"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://husky_description/meshes/dual_arm_bulkhead.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Attach bulkhead -->
  <joint name="dual_arm_bulkhead_joint" type="fixed">
    <parent link="base_link"/>
    <child link="dual_arm_bulkhead_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.225"/>
  </joint>
  <link name="husky_ptu_pan_link">
    <inertial>
      <mass value="0.65"/>
      <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>
    <visual>
      <material name="ptu_body_color">
        <color rgba="0.3 0.3 0.3 1.0"/>
      </material>
      <origin rpy="3.14 0 0" xyz="-0.0295 0.011 0"/>
      <geometry>
        <mesh filename="package://flir_ptu_description/meshes/flir-ptu-simple.stl"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.045 0.065 0.044"/>
      </geometry>
      <origin xyz="0 0.011 0"/>
    </collision>
    <collision>
      <geometry>
        <box size="0.046 0.044 0.044"/>
      </geometry>
      <origin xyz="-0.045 0.0215 0"/>
    </collision>
    <collision>
      <geometry>
        <cylinder length="0.015" radius="0.005"/>
      </geometry>
      <origin rpy="0 1.57 0" xyz="-0.074 0.021 0"/>
    </collision>
  </link>
  <gazebo reference="husky_ptu_pan_link">
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <link name="husky_ptu_tilt_link">
    <inertial>
      <mass value="0.65"/>
      <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>
    <visual>
      <material name="ptu_body_color">
        <color rgba="0.3 0.3 0.3 1.0"/>
      </material>
      <origin rpy="3.14 0 0" xyz="-0.0295 0.011 0"/>
      <geometry>
        <mesh filename="package://flir_ptu_description/meshes/flir-ptu-simple.stl"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.045 0.065 0.044"/>
      </geometry>
      <origin xyz="0 0.011 0"/>
    </collision>
    <collision>
      <geometry>
        <box size="0.046 0.044 0.044"/>
      </geometry>
      <origin xyz="-0.045 0.0215 0"/>
    </collision>
    <collision>
      <geometry>
        <cylinder length="0.015" radius="0.005"/>
      </geometry>
      <origin rpy="0 1.57 0" xyz="-0.074 0.021 0"/>
    </collision>
  </link>
  <gazebo reference="husky_ptu_tilt_link">
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <link name="husky_ptu_base_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>
  <link name="husky_ptu_tilted_link"/>
  <link name="husky_ptu_mount_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>
  <!-- There's an offset between the origin of the pan joint and the origin of 
         the overall device, which is between the mounting screws on its base. -->
  <joint name="husky_ptu_base" type="fixed">
    <parent link="husky_ptu_base_link"/>
    <child link="husky_ptu_pan_link"/>
    <origin xyz="0 -0.011 0.022"/>
  </joint>
  <!-- The pan joint -->
  <joint name="husky_ptu_pan" type="revolute">
    <parent link="husky_ptu_pan_link"/>
    <origin rpy="-1.5708 0 0" xyz="0 0 0.066"/>
    <child link="husky_ptu_tilt_link"/>
    <axis rpy="3.14159 0 0" xyz="0 -1 0"/>
    <limit effort="30" lower="-2.775" upper="2.775" velocity="1.0"/>
  </joint>
  <transmission name="husky_ptu_pan_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="husky_ptu_pan">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="husky_ptu_pan_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- The tilt joint -->
  <joint name="husky_ptu_tilt" type="revolute">
    <parent link="husky_ptu_tilt_link"/>
    <origin rpy="-1.5708 0 0" xyz="0 0 0"/>
    <child link="husky_ptu_tilted_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="30" lower="-0.82" upper="0.52" velocity="1.0"/>
  </joint>
  <transmission name="husky_ptu_tilt_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="husky_ptu_tilt">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="husky_ptu_tilt_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Fixed joint to provide a convenient attachment point for accessories. -->
  <joint name="husky_ptu_mount" type="fixed">
    <parent link="husky_ptu_tilted_link"/>
    <child link="husky_ptu_mount_link"/>
    <origin rpy="3.1416 0 0" xyz="0 0 -0.03912"/>
  </joint>
  <!-- Connect PTU to Husky bulkhead -->
  <joint name="husky_ptu_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.345 0.010414 0.19876"/>
    <!-- x-translation still needs to be measured -->
    <parent link="dual_arm_bulkhead_link"/>
    <child link="husky_ptu_base_link"/>
  </joint>
  <link name="l_ur5_arm_base_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/base.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/base.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="4.0"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
    </inertial>
  </link>
  <joint name="l_ur5_arm_shoulder_pan_joint" type="revolute">
    <parent link="l_ur5_arm_base_link"/>
    <child link="l_ur5_arm_shoulder_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/>
    <axis xyz="0 0 1"/>
    <limit effort="150.0" lower="-3.14159265" upper="3.14159265" velocity="3.15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="l_ur5_arm_shoulder_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/shoulder.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/shoulder.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="3.7"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" iyy="0.010267495893" iyz="0.0" izz="0.00666"/>
    </inertial>
  </link>
  <joint name="l_ur5_arm_shoulder_lift_joint" type="revolute">
    <parent link="l_ur5_arm_shoulder_link"/>
    <child link="l_ur5_arm_upper_arm_link"/>
    <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.13585 0.0"/>
    <axis xyz="0 1 0"/>
    <limit effort="150.0" lower="-3.14159265" upper="3.14159265" velocity="3.15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="l_ur5_arm_upper_arm_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/upperarm.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/upperarm.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="8.393"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.28"/>
      <inertia ixx="0.22689067591" ixy="0.0" ixz="0.0" iyy="0.22689067591" iyz="0.0" izz="0.0151074"/>
    </inertial>
  </link>
  <joint name="l_ur5_arm_elbow_joint" type="revolute">
    <parent link="l_ur5_arm_upper_arm_link"/>
    <child link="l_ur5_arm_forearm_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1197 0.425"/>
    <axis xyz="0 1 0"/>
    <limit effort="150.0" lower="-3.14159265" upper="3.14159265" velocity="3.15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="l_ur5_arm_forearm_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/forearm.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/forearm.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="2.275"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.25"/>
      <inertia ixx="0.049443313556" ixy="0.0" ixz="0.0" iyy="0.049443313556" iyz="0.0" izz="0.004095"/>
    </inertial>
  </link>
  <joint name="l_ur5_arm_wrist_1_joint" type="revolute">
    <parent link="l_ur5_arm_forearm_link"/>
    <child link="l_ur5_arm_wrist_1_link"/>
    <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.0 0.39225"/>
    <axis xyz="0 1 0"/>
    <limit effort="28.0" lower="-3.14159265" upper="3.14159265" velocity="3.2"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="l_ur5_arm_wrist_1_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/wrist1.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/wrist1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.219"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
    </inertial>
  </link>
  <joint name="l_ur5_arm_wrist_2_joint" type="revolute">
    <parent link="l_ur5_arm_wrist_1_link"/>
    <child link="l_ur5_arm_wrist_2_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="28.0" lower="-3.14159265" upper="3.14159265" velocity="3.2"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="l_ur5_arm_wrist_2_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/wrist2.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/wrist2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.219"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
    </inertial>
  </link>
  <joint name="l_ur5_arm_wrist_3_joint" type="revolute">
    <parent link="l_ur5_arm_wrist_2_link"/>
    <child link="l_ur5_arm_wrist_3_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
    <axis xyz="0 1 0"/>
    <limit effort="28.0" lower="-3.14159265" upper="3.14159265" velocity="3.2"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="l_ur5_arm_wrist_3_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/wrist3.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/wrist3.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1879"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.0171364731454" ixy="0.0" ixz="0.0" iyy="0.0171364731454" iyz="0.0" izz="0.033822"/>
    </inertial>
  </link>
  <joint name="l_ur5_arm_ee_fixed_joint" type="fixed">
    <parent link="l_ur5_arm_wrist_3_link"/>
    <child link="l_ur5_arm_ee_link"/>
    <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0823 0.0"/>
  </joint>
  <link name="l_ur5_arm_ee_link">
    <collision>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
      <origin rpy="0 0 0" xyz="-0.01 0 0"/>
    </collision>
  </link>
  <transmission name="l_ur5_arm_shoulder_pan_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="l_ur5_arm_shoulder_pan_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="l_ur5_arm_shoulder_pan_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="l_ur5_arm_shoulder_lift_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="l_ur5_arm_shoulder_lift_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="l_ur5_arm_shoulder_lift_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="l_ur5_arm_elbow_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="l_ur5_arm_elbow_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="l_ur5_arm_elbow_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="l_ur5_arm_wrist_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="l_ur5_arm_wrist_1_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="l_ur5_arm_wrist_1_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="l_ur5_arm_wrist_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="l_ur5_arm_wrist_2_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="l_ur5_arm_wrist_2_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="l_ur5_arm_wrist_3_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="l_ur5_arm_wrist_3_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="l_ur5_arm_wrist_3_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- nothing to do here at the moment -->
  <!-- ROS base_link to UR 'Base' Coordinates transform -->
  <link name="l_ur5_arm_base"/>
  <joint name="l_ur5_arm_base_link-base_fixed_joint" type="fixed">
    <!-- NOTE: this rotation is only needed as long as base_link itself is
                 not corrected wrt the real robot (ie: rotated over 180
                 degrees)
      -->
    <origin rpy="0 0 -3.14159265" xyz="0 0 0"/>
    <parent link="l_ur5_arm_base_link"/>
    <child link="l_ur5_arm_base"/>
  </joint>
  <!-- Frame coincident with all-zeros TCP on UR controller -->
  <link name="l_ur5_arm_tool0"/>
  <joint name="l_ur5_arm_wrist_3_link-tool0_fixed_joint" type="fixed">
    <origin rpy="-1.570796325 0 0" xyz="0 0.0823 0"/>
    <parent link="l_ur5_arm_wrist_3_link"/>
    <child link="l_ur5_arm_tool0"/>
  </joint>
  <link name="r_ur5_arm_base_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/base.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/base.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="4.0"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
    </inertial>
  </link>
  <joint name="r_ur5_arm_shoulder_pan_joint" type="revolute">
    <parent link="r_ur5_arm_base_link"/>
    <child link="r_ur5_arm_shoulder_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/>
    <axis xyz="0 0 1"/>
    <limit effort="150.0" lower="-3.14159265" upper="3.14159265" velocity="3.15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="r_ur5_arm_shoulder_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/shoulder.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/shoulder.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="3.7"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" iyy="0.010267495893" iyz="0.0" izz="0.00666"/>
    </inertial>
  </link>
  <joint name="r_ur5_arm_shoulder_lift_joint" type="revolute">
    <parent link="r_ur5_arm_shoulder_link"/>
    <child link="r_ur5_arm_upper_arm_link"/>
    <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.13585 0.0"/>
    <axis xyz="0 1 0"/>
    <limit effort="150.0" lower="-3.14159265" upper="3.14159265" velocity="3.15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="r_ur5_arm_upper_arm_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/upperarm.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/upperarm.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="8.393"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.28"/>
      <inertia ixx="0.22689067591" ixy="0.0" ixz="0.0" iyy="0.22689067591" iyz="0.0" izz="0.0151074"/>
    </inertial>
  </link>
  <joint name="r_ur5_arm_elbow_joint" type="revolute">
    <parent link="r_ur5_arm_upper_arm_link"/>
    <child link="r_ur5_arm_forearm_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1197 0.425"/>
    <axis xyz="0 1 0"/>
    <limit effort="150.0" lower="-3.14159265" upper="3.14159265" velocity="3.15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="r_ur5_arm_forearm_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/forearm.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/forearm.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="2.275"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.25"/>
      <inertia ixx="0.049443313556" ixy="0.0" ixz="0.0" iyy="0.049443313556" iyz="0.0" izz="0.004095"/>
    </inertial>
  </link>
  <joint name="r_ur5_arm_wrist_1_joint" type="revolute">
    <parent link="r_ur5_arm_forearm_link"/>
    <child link="r_ur5_arm_wrist_1_link"/>
    <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.0 0.39225"/>
    <axis xyz="0 1 0"/>
    <limit effort="28.0" lower="-3.14159265" upper="3.14159265" velocity="3.2"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="r_ur5_arm_wrist_1_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/wrist1.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/wrist1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.219"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
    </inertial>
  </link>
  <joint name="r_ur5_arm_wrist_2_joint" type="revolute">
    <parent link="r_ur5_arm_wrist_1_link"/>
    <child link="r_ur5_arm_wrist_2_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="28.0" lower="-3.14159265" upper="3.14159265" velocity="3.2"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="r_ur5_arm_wrist_2_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/wrist2.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/wrist2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.219"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
    </inertial>
  </link>
  <joint name="r_ur5_arm_wrist_3_joint" type="revolute">
    <parent link="r_ur5_arm_wrist_2_link"/>
    <child link="r_ur5_arm_wrist_3_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
    <axis xyz="0 1 0"/>
    <limit effort="28.0" lower="-3.14159265" upper="3.14159265" velocity="3.2"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="r_ur5_arm_wrist_3_link">
    <visual>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/wrist3.stl"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/collision/wrist3.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1879"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.0171364731454" ixy="0.0" ixz="0.0" iyy="0.0171364731454" iyz="0.0" izz="0.033822"/>
    </inertial>
  </link>
  <joint name="r_ur5_arm_ee_fixed_joint" type="fixed">
    <parent link="r_ur5_arm_wrist_3_link"/>
    <child link="r_ur5_arm_ee_link"/>
    <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0823 0.0"/>
  </joint>
  <link name="r_ur5_arm_ee_link">
    <collision>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
      <origin rpy="0 0 0" xyz="-0.01 0 0"/>
    </collision>
  </link>
  <transmission name="r_ur5_arm_shoulder_pan_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="r_ur5_arm_shoulder_pan_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="r_ur5_arm_shoulder_pan_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="r_ur5_arm_shoulder_lift_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="r_ur5_arm_shoulder_lift_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="r_ur5_arm_shoulder_lift_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="r_ur5_arm_elbow_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="r_ur5_arm_elbow_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="r_ur5_arm_elbow_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="r_ur5_arm_wrist_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="r_ur5_arm_wrist_1_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="r_ur5_arm_wrist_1_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="r_ur5_arm_wrist_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="r_ur5_arm_wrist_2_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="r_ur5_arm_wrist_2_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="r_ur5_arm_wrist_3_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="r_ur5_arm_wrist_3_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="r_ur5_arm_wrist_3_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- nothing to do here at the moment -->
  <!-- ROS base_link to UR 'Base' Coordinates transform -->
  <link name="r_ur5_arm_base"/>
  <joint name="r_ur5_arm_base_link-base_fixed_joint" type="fixed">
    <!-- NOTE: this rotation is only needed as long as base_link itself is
                 not corrected wrt the real robot (ie: rotated over 180
                 degrees)
      -->
    <origin rpy="0 0 -3.14159265" xyz="0 0 0"/>
    <parent link="r_ur5_arm_base_link"/>
    <child link="r_ur5_arm_base"/>
  </joint>
  <!-- Frame coincident with all-zeros TCP on UR controller -->
  <link name="r_ur5_arm_tool0"/>
  <joint name="r_ur5_arm_wrist_3_link-tool0_fixed_joint" type="fixed">
    <origin rpy="-1.570796325 0 0" xyz="0 0.0823 0"/>
    <parent link="r_ur5_arm_wrist_3_link"/>
    <child link="r_ur5_arm_tool0"/>
  </joint>
  <!-- Add laser specifically for this mode -->
  <link name="base_laser">
    <inertial>
      <mass value="1.1"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.00307156543804" ixy="0.0" ixz="0.0" iyy="0.00312849041527" iyz="0.0" izz="0.00196432421427"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <!-- Origin of this mesh is the focal point of the LIDAR. -->
        <mesh filename="package://lms1xx/meshes/sick-lms1xx.stl"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.05"/>
      <geometry>
        <box size="0.105 0.1 0.1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="base_laser">
    <turnGravityOff>true</turnGravityOff>
    <sensor name="base_laser" type="ray">
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>50</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-3.14</min_angle>
            <max_angle>3.14</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.001</stddev>
        </noise>
      </ray>
      <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_laser">
        <topicName>scan</topicName>
        <frameName>base_laser</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <!-- This offset is from the base of the bracket to the LIDAR's focal point. -->
  <joint name="base_laser_joint" type="fixed">
    <origin rpy="3.14159 0 0" xyz="0.465 0 -0.085"/>
    <parent link="dual_arm_bulkhead_link"/>
    <child link="base_laser"/>
  </joint>
  <!-- Add bumblebee camera -->
  <link name="bumblebee2">
    <visual>
      <origin rpy="-1.5707 0 -1.5707" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pointgrey_camera_description/meshes/pointgrey_bumblebee2.stl"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.04740 0.157 0.036"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.342"/>
      <inertia ixx="0.00010096866" ixy="0" ixz="0" iyy="0.00076652916" iyz="0" izz="0.0007394325"/>
    </inertial>
  </link>
  <link name="bumblebee2_optical">
    <inertial>
      <mass value="1E-5"/>
      <inertia ixx="1E-10" ixy="0" ixz="0" iyy="1E-10" iyz="0" izz="1E-10"/>
    </inertial>
  </link>
  <joint name="bumblebee2_optical_joint" type="fixed">
    <origin rpy="-1.570796 0 -1.570796" xyz="0 0 0"/>
    <parent link="bumblebee2"/>
    <child link="bumblebee2_optical"/>
  </joint>
  <gazebo reference="bumblebee2_optical">
    <sensor name="stereo_camera" type="multicamera">
      <update_rate>20</update_rate>
      <camera name="left">
        <pose>0 0 0 0 -1.5707 1.5707</pose>
        <horizontal_fov>1.15191730632</horizontal_fov>
        <image>
          <width>1024</width>
          <height>768</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.5</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <camera name="right">
        <pose>0.12 0 0 0 -1.5707 1.5707</pose>
        <horizontal_fov>1.15191730632</horizontal_fov>
        <image>
          <width>1024</width>
          <height>768</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.5</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <plugin filename="libgazebo_ros_multicamera.so" name="stereo_camera_controller">
        <alwaysOn>true</alwaysOn>
        <cameraName>bumblebee2</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>bumblebee2_optical</frameName>
        <hackBaseline>0.12</hackBaseline>
        <!-- NOTE: Distortion is currently unused -->
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
  <joint name="bumblebee2_mount_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.02224"/>
    <parent link="husky_ptu_mount_link"/>
    <child link="bumblebee2"/>
  </joint>
  <!-- Add force torque model FT300 -->
  <!-- mount the fts to the robot -->
  <joint name="l_robotiq_fts300_fts_fix" type="fixed">
    <!--In most cases origin would be flange adapter plate: <origin xyz="0 0 0.009" rpy="0 0 0"/> /-->
    <origin rpy="0 1.57 0" xyz="0 0 0"/>
    <parent link="l_ur5_arm_ee_link"/>
    <child link="l_robotiq_fts300_fts_robotside"/>
  </joint>
  <link name="l_robotiq_fts300_fts_robotside">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0.01875"/>
      <mass value="0.65"/>
      <inertia ixx="0.000661171875" ixy="0" ixz="0" iyy="0.000661171875" iyz="0" izz="0.00117"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_force_torque_sensor/meshes/visual/robotiq_fts300.stl"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_force_torque_sensor/meshes/collision/robotiq_fts300.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- virtual measurement frame -->
  <joint name="l_robotiq_fts300_measurment_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.01625"/>
    <parent link="l_robotiq_fts300_fts_robotside"/>
    <child link="l_robotiq_fts300_robotiq_force_torque_frame_id"/>
  </joint>
  <link name="l_robotiq_fts300_robotiq_force_torque_frame_id"/>
  <!-- toolside frame -->
  <joint name="l_robotiq_fts300_toolside_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.0375"/>
    <parent link="l_robotiq_fts300_fts_robotside"/>
    <child link="l_robotiq_fts300_fts_toolside"/>
  </joint>
  <link name="l_robotiq_fts300_fts_toolside"/>
  <!-- mount the fts to the robot -->
  <joint name="r_robotiq_fts300_fts_fix" type="fixed">
    <!--In most cases origin would be flange adapter plate: <origin xyz="0 0 0.009" rpy="0 0 0"/> /-->
    <origin rpy="0 1.57 0" xyz="0 0 0"/>
    <parent link="r_ur5_arm_ee_link"/>
    <child link="r_robotiq_fts300_fts_robotside"/>
  </joint>
  <link name="r_robotiq_fts300_fts_robotside">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0.01875"/>
      <mass value="0.65"/>
      <inertia ixx="0.000661171875" ixy="0" ixz="0" iyy="0.000661171875" iyz="0" izz="0.00117"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_force_torque_sensor/meshes/visual/robotiq_fts300.stl"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_force_torque_sensor/meshes/collision/robotiq_fts300.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- virtual measurement frame -->
  <joint name="r_robotiq_fts300_measurment_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.01625"/>
    <parent link="r_robotiq_fts300_fts_robotside"/>
    <child link="r_robotiq_fts300_robotiq_force_torque_frame_id"/>
  </joint>
  <link name="r_robotiq_fts300_robotiq_force_torque_frame_id"/>
  <!-- toolside frame -->
  <joint name="r_robotiq_fts300_toolside_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.0375"/>
    <parent link="r_robotiq_fts300_fts_robotside"/>
    <child link="r_robotiq_fts300_fts_toolside"/>
  </joint>
  <link name="r_robotiq_fts300_fts_toolside"/>
  <!-- joint attaching parent link to robotiq hand -->
  <joint name="l_robotiq_hand_joint" type="fixed">
    <origin rpy="1.57 0 1.0" xyz="0 0 0.05398"/>
    <parent link="l_robotiq_fts300_fts_toolside"/>
    <child link="l_palm"/>
  </joint>
  <!-- Insert the fingers -->
  <!-- Link list -->
  <!-- Link 0 -->
  <link name="l_finger_1_link_0">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_0.STL"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_0.STL"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <origin xyz="-0.01 0 0"/>
      <inertia ixx="0.000018" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_1_link_0">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 1 -->
  <link name="l_finger_1_link_1">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0.05717 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_1.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 1.5708" xyz="0 0.05717 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_1.STL"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <origin rpy="0 0 1.5708" xyz="0.00053 0.03218 0"/>
      <mass value="0.047"/>
      <inertia ixx="0.00000499332" ixy="0.00000091312" ixz="-0.00000000120" iyy="0.00006434531" iyz="-0.00000000007" izz="0.00006061917"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_1_link_1">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 2 -->
  <link name="l_finger_1_link_2">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0.03810 0.0075"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_2.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 1.5708" xyz="0 0.03810 0.0075"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_2.STL"/>
      </geometry>
      <material name="yellow"/>
    </collision>
    <inertial>
      <origin rpy="0 0 1.5708" xyz="0.00048 0.02167 0.00018"/>
      <mass value="0.03284"/>
      <inertia ixx="0.00000348008" ixy="0.00000031940" ixz="0.00000013041" iyy="0.00002248123" iyz="0.00000000291" izz="0.00001986346"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_1_link_2">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 3 -->
  <link name="l_finger_1_link_3">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_3.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_3.STL"/>
      </geometry>
      <material name="yellow"/>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01626 0.00049 0.0002"/>
      <mass value="0.03354"/>
      <inertia ixx="0.00000305151" ixy="0.00000089792" ixz="0.00000000249" iyy="0.00001427978" iyz="-0.00000001191" izz="0.00001264126"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_1_link_3">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link proximal actuating hinge -->
  <link name="l_finger_1_link_proximal_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0.01429 0 0"/>
      <geometry>
        <box size="0.02858 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01429 0 0"/>
      <geometry>
        <box size="0.02858 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0012 -0.00162 -0.00161"/>
      <mass value="0.02647"/>
      <inertia ixx="0.00000156447" ixy="0.000000236226" ixz="-0.00000014862" iyy="0.00000221068" iyz="-0.00000014369" izz="0.00000323175"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_1_link_proximal_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paraproximal actuating hinge -->
  <link name="l_finger_1_link_paraproximal_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0.01143 0 0"/>
      <geometry>
        <box size="0.02286 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01143 0 0"/>
      <geometry>
        <box size="0.02286 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0064 0 0"/>
      <!-- copied from proximal_actuating_hinge -->
      <mass value="0.00362"/>
      <inertia ixx="0.00000012043" ixy="0" ixz="0" iyy="0.00000053588" iyz="0" izz="0.00000065322"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_1_link_paraproximal_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link proximal actuating bar -->
  <link name="l_finger_1_link_proximal_actuating_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.026789 0 0"/>
      <geometry>
        <box size="0.05398 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.026789 0 0"/>
      <geometry>
        <box size="0.05398 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.02699 -0.00122 0"/>
      <mass value="0.00659"/>
      <inertia ixx="0.00000008348" ixy="-0.0000002179" ixz="0.00000000134" iyy="0.00000719076" iyz="0" izz="0.00000718272"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_1_link_proximal_actuating_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paraproximal bar -->
  <link name="l_finger_1_link_paraproximal_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.02885 0 0"/>
      <geometry>
        <box size="0.05770 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.02885 0 0"/>
      <geometry>
        <box size="0.05770 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.02858 -0.00129 0"/>
      <mass value="0.00732"/>
      <inertia ixx="0.00000009065" ixy="-0.00000027047" ixz="0.00000000142" iyy="0.00000878034" iyz="0" izz="0.00000877558"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_1_link_paraproximal_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median actuating hinge -->
  <link name="l_finger_1_link_median_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0092 0.00514 0"/>
      <mass value="0.00359"/>
      <inertia ixx="0.00000024735" ixy="0.00000023619" ixz="0" iyy="0.00000047681" iyz="0" izz="0.00000071813"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_1_link_median_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median bar -->
  <link name="l_finger_1_link_median_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01905 -0.00107 0"/>
      <mass value="0.00538"/>
      <inertia ixx="0.00000006639" ixy="-0.00000010996" ixz="0.00000000094" iyy="0.00000305318" iyz="0" izz="0.00000304657"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_1_link_median_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paramedian hinge -->
  <link name="l_finger_1_link_paramedian_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01025 0.00046 0"/>
      <mass value="0.00276"/>
      <inertia ixx="0.00000004213" ixy="-0.00000001491" ixz="0" iyy="0.00000050258" iyz="0" izz="0.00000054007"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_1_link_paramedian_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median bar underactuated -->
  <link name="l_finger_1_link_median_bar_underactuated">
    <visual>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01905 -0.00107 0"/>
      <mass value="0.00538"/>
      <inertia ixx="0.00000006639" ixy="-0.00000010996" ixz="-0.00000000094" iyy="0.00000305318" iyz="0" izz="0.00000304657"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_1_link_median_bar_underactuated">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paradistal hinge -->
  <link name="l_finger_1_link_paradistal_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 0.02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.00548 0 0"/>
      <mass value="0.00158"/>
      <inertia ixx="0.00000001907" ixy="0" ixz="0" iyy="0.00000010970" iyz="0" izz="0.00000012610"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_1_link_paradistal_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- End of link list -->
  <!-- Joint list -->
  <!-- Joint proximal actuating hinge -->
  <joint name="l_finger_1_joint_proximal_actuating_hinge" type="revolute">
    <parent link="l_finger_1_link_0"/>
    <child link="l_finger_1_link_proximal_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -2.0906" xyz="0 0 0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_1_joint_proximal_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paraproximal actuating hinge -->
  <joint name="l_finger_1_joint_paraproximal_actuating_hinge" type="revolute">
    <parent link="l_finger_1_link_0"/>
    <child link="l_finger_1_link_paraproximal_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -2.0906" xyz="0 0 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_1_joint_paraproximal_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint proximal actuating bar -->
  <joint name="l_finger_1_joint_proximal_actuating_bar" type="revolute">
    <parent link="l_finger_1_link_proximal_actuating_hinge"/>
    <child link="l_finger_1_link_proximal_actuating_bar"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 1.8616" xyz="0.02858 0 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_1_joint_proximal_actuating_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paraproximal bar -->
  <joint name="l_finger_1_joint_paraproximal_bar" type="revolute">
    <parent link="l_finger_1_link_paraproximal_actuating_hinge"/>
    <child link="l_finger_1_link_paraproximal_bar"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 1.57" xyz="0.02286 0 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_1_joint_paraproximal_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint median actuating hinge -->
  <joint name="l_finger_1_joint_median_actuating_hinge" type="revolute">
    <parent link="l_finger_1_link_1"/>
    <child link="l_finger_1_link_median_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -0.37" xyz="0 0.05715 0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_1_joint_median_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paramedian hinge -->
  <joint name="l_finger_1_joint_paramedian_hinge" type="revolute">
    <parent link="l_finger_1_link_1"/>
    <child link="l_finger_1_link_paramedian_hinge"/>
    <axis xyz="0 0 1"/>
    <!-- -0.37 is hand picked, 0.9719 is hand calculated from atan(0.02381/0.01625) -->
    <origin rpy="0 0 0" xyz="0 0.05715 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_1_joint_paramedian_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint median actuating hinge median bar -->
  <joint name="l_finger_1_joint_median_actuating_hinge_median_bar" type="revolute">
    <parent link="l_finger_1_link_median_actuating_hinge"/>
    <child link="l_finger_1_link_median_bar"/>
    <axis xyz="0 0 1"/>
    <!-- hand calc 90 deg + atan(0.01516/0.01625) -->
    <origin rpy="0 0 2.3215" xyz="0.01625 0.01516 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_1_joint_median_actuating_hinge_median_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paradistal hinge -->
  <joint name="l_finger_1_joint_paradistal_hinge" type="revolute">
    <parent link="l_finger_1_link_2"/>
    <child link="l_finger_1_link_paradistal_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0.039 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_1_joint_paradistal_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paramedian hinge median bar underactuated -->
  <joint name="l_finger_1_joint_paramedian_hinge_median_bar_underactuated" type="revolute">
    <parent link="l_finger_1_link_paramedian_hinge"/>
    <child link="l_finger_1_link_median_bar_underactuated"/>
    <axis xyz="0 0 1"/>
    <!-- hand calc 90 deg + atan(0.01227/0.00329) -->
    <origin rpy="0 0 1.5708" xyz="0.01227 0.00329 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_1_joint_paramedian_hinge_median_bar_underactuated">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 1 -->
  <joint name="l_finger_1_joint_1" type="revolute">
    <origin rpy="0 0 -2.0906" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <parent link="l_finger_1_link_0"/>
    <child link="l_finger_1_link_1"/>
    <dynamics damping="1" friction="0"/>
    <!-- Limits: [0deg.,70deg.] -->
    <limit effort="60" lower="0" upper="1.2217" velocity="-1"/>
  </joint>
  <gazebo reference="l_finger_1_joint_1">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 2 -->
  <joint name="l_finger_1_joint_2" type="revolute">
    <parent link="l_finger_1_link_1"/>
    <child link="l_finger_1_link_2"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0.05715 0"/>
    <!-- Limits: [0deg.,90deg.] -->
    <limit effort="60" lower="0" upper="1.5708" velocity="-1"/>
  </joint>
  <gazebo reference="l_finger_1_joint_2">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 3 -->
  <joint name="l_finger_1_joint_3" type="revolute">
    <parent link="l_finger_1_link_2"/>
    <child link="l_finger_1_link_3"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 2.0906" xyz="0 0.039 0"/>
    <!-- Limits: [-38deg.,60deg.] -->
    <limit effort="60" lower="-0.6632" upper="1.0471" velocity="-1"/>
  </joint>
  <gazebo reference="l_finger_1_joint_3">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- end of joint list -->
  <!-- coupling joints in sdf -->
  <gazebo>
    <joint name="l_finger_1_link_proximal_actuating_bar_median_actuating_hinge_couple" type="revolute">
      <parent>l_finger_1_link_median_actuating_hinge</parent>
      <child>l_finger_1_link_proximal_actuating_bar</child>
      <pose>0.05398 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_1_link_proximal_actuating_bar_median_actuating_hinge_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="l_finger_1_link_median_bar_link_3_couple" type="revolute">
      <parent>l_finger_1_link_3</parent>
      <child>l_finger_1_link_median_bar</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_1_link_median_bar_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="l_finger_1_link_paramedian_bar_paradistal_hinge_couple" type="revolute">
      <parent>l_finger_1_link_paradistal_hinge</parent>
      <child>l_finger_1_link_median_bar_underactuated</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_1_link_median_bar_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="l_finger_1_link_paradistal_hinge_link_3_couple" type="revolute">
      <parent>l_finger_1_link_paradistal_hinge</parent>
      <child>l_finger_1_link_3</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_1_link_paradistal_hinge_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="l_finger_1_link_paraproximal_bar_paramedian_hinge_couple" type="revolute">
      <parent>l_finger_1_link_paramedian_hinge</parent>
      <child>l_finger_1_link_paraproximal_bar</child>
      <pose>0.05770 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_1_link_paraproximal_bar_paramedian_hinge_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Link list -->
  <!-- Link 0 -->
  <link name="l_finger_2_link_0">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_0.STL"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_0.STL"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <origin xyz="-0.01 0 0"/>
      <inertia ixx="0.000018" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_2_link_0">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 1 -->
  <link name="l_finger_2_link_1">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0.05717 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_1.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 1.5708" xyz="0 0.05717 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_1.STL"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <origin rpy="0 0 1.5708" xyz="0.00053 0.03218 0"/>
      <mass value="0.047"/>
      <inertia ixx="0.00000499332" ixy="0.00000091312" ixz="-0.00000000120" iyy="0.00006434531" iyz="-0.00000000007" izz="0.00006061917"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_2_link_1">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 2 -->
  <link name="l_finger_2_link_2">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0.03810 0.0075"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_2.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 1.5708" xyz="0 0.03810 0.0075"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_2.STL"/>
      </geometry>
      <material name="yellow"/>
    </collision>
    <inertial>
      <origin rpy="0 0 1.5708" xyz="0.00048 0.02167 0.00018"/>
      <mass value="0.03284"/>
      <inertia ixx="0.00000348008" ixy="0.00000031940" ixz="0.00000013041" iyy="0.00002248123" iyz="0.00000000291" izz="0.00001986346"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_2_link_2">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 3 -->
  <link name="l_finger_2_link_3">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_3.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_3.STL"/>
      </geometry>
      <material name="yellow"/>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01626 0.00049 0.0002"/>
      <mass value="0.03354"/>
      <inertia ixx="0.00000305151" ixy="0.00000089792" ixz="0.00000000249" iyy="0.00001427978" iyz="-0.00000001191" izz="0.00001264126"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_2_link_3">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link proximal actuating hinge -->
  <link name="l_finger_2_link_proximal_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0.01429 0 0"/>
      <geometry>
        <box size="0.02858 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01429 0 0"/>
      <geometry>
        <box size="0.02858 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0012 -0.00162 -0.00161"/>
      <mass value="0.02647"/>
      <inertia ixx="0.00000156447" ixy="0.000000236226" ixz="-0.00000014862" iyy="0.00000221068" iyz="-0.00000014369" izz="0.00000323175"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_2_link_proximal_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paraproximal actuating hinge -->
  <link name="l_finger_2_link_paraproximal_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0.01143 0 0"/>
      <geometry>
        <box size="0.02286 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01143 0 0"/>
      <geometry>
        <box size="0.02286 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0064 0 0"/>
      <!-- copied from proximal_actuating_hinge -->
      <mass value="0.00362"/>
      <inertia ixx="0.00000012043" ixy="0" ixz="0" iyy="0.00000053588" iyz="0" izz="0.00000065322"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_2_link_paraproximal_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link proximal actuating bar -->
  <link name="l_finger_2_link_proximal_actuating_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.026789 0 0"/>
      <geometry>
        <box size="0.05398 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.026789 0 0"/>
      <geometry>
        <box size="0.05398 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.02699 -0.00122 0"/>
      <mass value="0.00659"/>
      <inertia ixx="0.00000008348" ixy="-0.0000002179" ixz="0.00000000134" iyy="0.00000719076" iyz="0" izz="0.00000718272"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_2_link_proximal_actuating_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paraproximal bar -->
  <link name="l_finger_2_link_paraproximal_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.02885 0 0"/>
      <geometry>
        <box size="0.05770 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.02885 0 0"/>
      <geometry>
        <box size="0.05770 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.02858 -0.00129 0"/>
      <mass value="0.00732"/>
      <inertia ixx="0.00000009065" ixy="-0.00000027047" ixz="0.00000000142" iyy="0.00000878034" iyz="0" izz="0.00000877558"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_2_link_paraproximal_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median actuating hinge -->
  <link name="l_finger_2_link_median_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0092 0.00514 0"/>
      <mass value="0.00359"/>
      <inertia ixx="0.00000024735" ixy="0.00000023619" ixz="0" iyy="0.00000047681" iyz="0" izz="0.00000071813"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_2_link_median_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median bar -->
  <link name="l_finger_2_link_median_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01905 -0.00107 0"/>
      <mass value="0.00538"/>
      <inertia ixx="0.00000006639" ixy="-0.00000010996" ixz="0.00000000094" iyy="0.00000305318" iyz="0" izz="0.00000304657"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_2_link_median_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paramedian hinge -->
  <link name="l_finger_2_link_paramedian_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01025 0.00046 0"/>
      <mass value="0.00276"/>
      <inertia ixx="0.00000004213" ixy="-0.00000001491" ixz="0" iyy="0.00000050258" iyz="0" izz="0.00000054007"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_2_link_paramedian_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median bar underactuated -->
  <link name="l_finger_2_link_median_bar_underactuated">
    <visual>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01905 -0.00107 0"/>
      <mass value="0.00538"/>
      <inertia ixx="0.00000006639" ixy="-0.00000010996" ixz="-0.00000000094" iyy="0.00000305318" iyz="0" izz="0.00000304657"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_2_link_median_bar_underactuated">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paradistal hinge -->
  <link name="l_finger_2_link_paradistal_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 0.02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.00548 0 0"/>
      <mass value="0.00158"/>
      <inertia ixx="0.00000001907" ixy="0" ixz="0" iyy="0.00000010970" iyz="0" izz="0.00000012610"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_2_link_paradistal_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- End of link list -->
  <!-- Joint list -->
  <!-- Joint proximal actuating hinge -->
  <joint name="l_finger_2_joint_proximal_actuating_hinge" type="revolute">
    <parent link="l_finger_2_link_0"/>
    <child link="l_finger_2_link_proximal_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -2.0906" xyz="0 0 0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_2_joint_proximal_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paraproximal actuating hinge -->
  <joint name="l_finger_2_joint_paraproximal_actuating_hinge" type="revolute">
    <parent link="l_finger_2_link_0"/>
    <child link="l_finger_2_link_paraproximal_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -2.0906" xyz="0 0 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_2_joint_paraproximal_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint proximal actuating bar -->
  <joint name="l_finger_2_joint_proximal_actuating_bar" type="revolute">
    <parent link="l_finger_2_link_proximal_actuating_hinge"/>
    <child link="l_finger_2_link_proximal_actuating_bar"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 1.8616" xyz="0.02858 0 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_2_joint_proximal_actuating_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paraproximal bar -->
  <joint name="l_finger_2_joint_paraproximal_bar" type="revolute">
    <parent link="l_finger_2_link_paraproximal_actuating_hinge"/>
    <child link="l_finger_2_link_paraproximal_bar"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 1.57" xyz="0.02286 0 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_2_joint_paraproximal_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint median actuating hinge -->
  <joint name="l_finger_2_joint_median_actuating_hinge" type="revolute">
    <parent link="l_finger_2_link_1"/>
    <child link="l_finger_2_link_median_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -0.37" xyz="0 0.05715 0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_2_joint_median_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paramedian hinge -->
  <joint name="l_finger_2_joint_paramedian_hinge" type="revolute">
    <parent link="l_finger_2_link_1"/>
    <child link="l_finger_2_link_paramedian_hinge"/>
    <axis xyz="0 0 1"/>
    <!-- -0.37 is hand picked, 0.9719 is hand calculated from atan(0.02381/0.01625) -->
    <origin rpy="0 0 0" xyz="0 0.05715 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_2_joint_paramedian_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint median actuating hinge median bar -->
  <joint name="l_finger_2_joint_median_actuating_hinge_median_bar" type="revolute">
    <parent link="l_finger_2_link_median_actuating_hinge"/>
    <child link="l_finger_2_link_median_bar"/>
    <axis xyz="0 0 1"/>
    <!-- hand calc 90 deg + atan(0.01516/0.01625) -->
    <origin rpy="0 0 2.3215" xyz="0.01625 0.01516 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_2_joint_median_actuating_hinge_median_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paradistal hinge -->
  <joint name="l_finger_2_joint_paradistal_hinge" type="revolute">
    <parent link="l_finger_2_link_2"/>
    <child link="l_finger_2_link_paradistal_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0.039 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_2_joint_paradistal_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paramedian hinge median bar underactuated -->
  <joint name="l_finger_2_joint_paramedian_hinge_median_bar_underactuated" type="revolute">
    <parent link="l_finger_2_link_paramedian_hinge"/>
    <child link="l_finger_2_link_median_bar_underactuated"/>
    <axis xyz="0 0 1"/>
    <!-- hand calc 90 deg + atan(0.01227/0.00329) -->
    <origin rpy="0 0 1.5708" xyz="0.01227 0.00329 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_2_joint_paramedian_hinge_median_bar_underactuated">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 1 -->
  <joint name="l_finger_2_joint_1" type="revolute">
    <origin rpy="0 0 -2.0906" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <parent link="l_finger_2_link_0"/>
    <child link="l_finger_2_link_1"/>
    <dynamics damping="1" friction="0"/>
    <!-- Limits: [0deg.,70deg.] -->
    <limit effort="60" lower="0" upper="1.2217" velocity="-1"/>
  </joint>
  <gazebo reference="l_finger_2_joint_1">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 2 -->
  <joint name="l_finger_2_joint_2" type="revolute">
    <parent link="l_finger_2_link_1"/>
    <child link="l_finger_2_link_2"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0.05715 0"/>
    <!-- Limits: [0deg.,90deg.] -->
    <limit effort="60" lower="0" upper="1.5708" velocity="-1"/>
  </joint>
  <gazebo reference="l_finger_2_joint_2">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 3 -->
  <joint name="l_finger_2_joint_3" type="revolute">
    <parent link="l_finger_2_link_2"/>
    <child link="l_finger_2_link_3"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 2.0906" xyz="0 0.039 0"/>
    <!-- Limits: [-38deg.,60deg.] -->
    <limit effort="60" lower="-0.6632" upper="1.0471" velocity="-1"/>
  </joint>
  <gazebo reference="l_finger_2_joint_3">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- end of joint list -->
  <!-- coupling joints in sdf -->
  <gazebo>
    <joint name="l_finger_2_link_proximal_actuating_bar_median_actuating_hinge_couple" type="revolute">
      <parent>l_finger_2_link_median_actuating_hinge</parent>
      <child>l_finger_2_link_proximal_actuating_bar</child>
      <pose>0.05398 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_2_link_proximal_actuating_bar_median_actuating_hinge_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="l_finger_2_link_median_bar_link_3_couple" type="revolute">
      <parent>l_finger_2_link_3</parent>
      <child>l_finger_2_link_median_bar</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_2_link_median_bar_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="l_finger_2_link_paramedian_bar_paradistal_hinge_couple" type="revolute">
      <parent>l_finger_2_link_paradistal_hinge</parent>
      <child>l_finger_2_link_median_bar_underactuated</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_2_link_median_bar_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="l_finger_2_link_paradistal_hinge_link_3_couple" type="revolute">
      <parent>l_finger_2_link_paradistal_hinge</parent>
      <child>l_finger_2_link_3</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_2_link_paradistal_hinge_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="l_finger_2_link_paraproximal_bar_paramedian_hinge_couple" type="revolute">
      <parent>l_finger_2_link_paramedian_hinge</parent>
      <child>l_finger_2_link_paraproximal_bar</child>
      <pose>0.05770 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_2_link_paraproximal_bar_paramedian_hinge_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Link list -->
  <!-- Link 0 -->
  <link name="l_finger_middle_link_0">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_0.STL"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_0.STL"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <origin xyz="-0.01 0 0"/>
      <inertia ixx="0.000018" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_middle_link_0">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 1 -->
  <link name="l_finger_middle_link_1">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0.05717 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_1.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 1.5708" xyz="0 0.05717 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_1.STL"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <origin rpy="0 0 1.5708" xyz="0.00053 0.03218 0"/>
      <mass value="0.047"/>
      <inertia ixx="0.00000499332" ixy="0.00000091312" ixz="-0.00000000120" iyy="0.00006434531" iyz="-0.00000000007" izz="0.00006061917"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_middle_link_1">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 2 -->
  <link name="l_finger_middle_link_2">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0.03810 0.0075"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_2.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 1.5708" xyz="0 0.03810 0.0075"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_2.STL"/>
      </geometry>
      <material name="yellow"/>
    </collision>
    <inertial>
      <origin rpy="0 0 1.5708" xyz="0.00048 0.02167 0.00018"/>
      <mass value="0.03284"/>
      <inertia ixx="0.00000348008" ixy="0.00000031940" ixz="0.00000013041" iyy="0.00002248123" iyz="0.00000000291" izz="0.00001986346"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_middle_link_2">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 3 -->
  <link name="l_finger_middle_link_3">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_3.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_3.STL"/>
      </geometry>
      <material name="yellow"/>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01626 0.00049 0.0002"/>
      <mass value="0.03354"/>
      <inertia ixx="0.00000305151" ixy="0.00000089792" ixz="0.00000000249" iyy="0.00001427978" iyz="-0.00000001191" izz="0.00001264126"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_middle_link_3">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link proximal actuating hinge -->
  <link name="l_finger_middle_link_proximal_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0.01429 0 0"/>
      <geometry>
        <box size="0.02858 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01429 0 0"/>
      <geometry>
        <box size="0.02858 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0012 -0.00162 -0.00161"/>
      <mass value="0.02647"/>
      <inertia ixx="0.00000156447" ixy="0.000000236226" ixz="-0.00000014862" iyy="0.00000221068" iyz="-0.00000014369" izz="0.00000323175"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_middle_link_proximal_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paraproximal actuating hinge -->
  <link name="l_finger_middle_link_paraproximal_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0.01143 0 0"/>
      <geometry>
        <box size="0.02286 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01143 0 0"/>
      <geometry>
        <box size="0.02286 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0064 0 0"/>
      <!-- copied from proximal_actuating_hinge -->
      <mass value="0.00362"/>
      <inertia ixx="0.00000012043" ixy="0" ixz="0" iyy="0.00000053588" iyz="0" izz="0.00000065322"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_middle_link_paraproximal_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link proximal actuating bar -->
  <link name="l_finger_middle_link_proximal_actuating_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.026789 0 0"/>
      <geometry>
        <box size="0.05398 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.026789 0 0"/>
      <geometry>
        <box size="0.05398 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.02699 -0.00122 0"/>
      <mass value="0.00659"/>
      <inertia ixx="0.00000008348" ixy="-0.0000002179" ixz="0.00000000134" iyy="0.00000719076" iyz="0" izz="0.00000718272"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_middle_link_proximal_actuating_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paraproximal bar -->
  <link name="l_finger_middle_link_paraproximal_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.02885 0 0"/>
      <geometry>
        <box size="0.05770 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.02885 0 0"/>
      <geometry>
        <box size="0.05770 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.02858 -0.00129 0"/>
      <mass value="0.00732"/>
      <inertia ixx="0.00000009065" ixy="-0.00000027047" ixz="0.00000000142" iyy="0.00000878034" iyz="0" izz="0.00000877558"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_middle_link_paraproximal_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median actuating hinge -->
  <link name="l_finger_middle_link_median_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0092 0.00514 0"/>
      <mass value="0.00359"/>
      <inertia ixx="0.00000024735" ixy="0.00000023619" ixz="0" iyy="0.00000047681" iyz="0" izz="0.00000071813"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_middle_link_median_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median bar -->
  <link name="l_finger_middle_link_median_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01905 -0.00107 0"/>
      <mass value="0.00538"/>
      <inertia ixx="0.00000006639" ixy="-0.00000010996" ixz="0.00000000094" iyy="0.00000305318" iyz="0" izz="0.00000304657"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_middle_link_median_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paramedian hinge -->
  <link name="l_finger_middle_link_paramedian_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01025 0.00046 0"/>
      <mass value="0.00276"/>
      <inertia ixx="0.00000004213" ixy="-0.00000001491" ixz="0" iyy="0.00000050258" iyz="0" izz="0.00000054007"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_middle_link_paramedian_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median bar underactuated -->
  <link name="l_finger_middle_link_median_bar_underactuated">
    <visual>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01905 -0.00107 0"/>
      <mass value="0.00538"/>
      <inertia ixx="0.00000006639" ixy="-0.00000010996" ixz="-0.00000000094" iyy="0.00000305318" iyz="0" izz="0.00000304657"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_middle_link_median_bar_underactuated">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paradistal hinge -->
  <link name="l_finger_middle_link_paradistal_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 0.02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.00548 0 0"/>
      <mass value="0.00158"/>
      <inertia ixx="0.00000001907" ixy="0" ixz="0" iyy="0.00000010970" iyz="0" izz="0.00000012610"/>
    </inertial>
  </link>
  <gazebo reference="l_finger_middle_link_paradistal_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- End of link list -->
  <!-- Joint list -->
  <!-- Joint proximal actuating hinge -->
  <joint name="l_finger_middle_joint_proximal_actuating_hinge" type="revolute">
    <parent link="l_finger_middle_link_0"/>
    <child link="l_finger_middle_link_proximal_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -2.0906" xyz="0 0 0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_middle_joint_proximal_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paraproximal actuating hinge -->
  <joint name="l_finger_middle_joint_paraproximal_actuating_hinge" type="revolute">
    <parent link="l_finger_middle_link_0"/>
    <child link="l_finger_middle_link_paraproximal_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -2.0906" xyz="0 0 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_middle_joint_paraproximal_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint proximal actuating bar -->
  <joint name="l_finger_middle_joint_proximal_actuating_bar" type="revolute">
    <parent link="l_finger_middle_link_proximal_actuating_hinge"/>
    <child link="l_finger_middle_link_proximal_actuating_bar"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 1.8616" xyz="0.02858 0 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_middle_joint_proximal_actuating_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paraproximal bar -->
  <joint name="l_finger_middle_joint_paraproximal_bar" type="revolute">
    <parent link="l_finger_middle_link_paraproximal_actuating_hinge"/>
    <child link="l_finger_middle_link_paraproximal_bar"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 1.57" xyz="0.02286 0 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_middle_joint_paraproximal_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint median actuating hinge -->
  <joint name="l_finger_middle_joint_median_actuating_hinge" type="revolute">
    <parent link="l_finger_middle_link_1"/>
    <child link="l_finger_middle_link_median_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -0.37" xyz="0 0.05715 0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_middle_joint_median_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paramedian hinge -->
  <joint name="l_finger_middle_joint_paramedian_hinge" type="revolute">
    <parent link="l_finger_middle_link_1"/>
    <child link="l_finger_middle_link_paramedian_hinge"/>
    <axis xyz="0 0 1"/>
    <!-- -0.37 is hand picked, 0.9719 is hand calculated from atan(0.02381/0.01625) -->
    <origin rpy="0 0 0" xyz="0 0.05715 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_middle_joint_paramedian_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint median actuating hinge median bar -->
  <joint name="l_finger_middle_joint_median_actuating_hinge_median_bar" type="revolute">
    <parent link="l_finger_middle_link_median_actuating_hinge"/>
    <child link="l_finger_middle_link_median_bar"/>
    <axis xyz="0 0 1"/>
    <!-- hand calc 90 deg + atan(0.01516/0.01625) -->
    <origin rpy="0 0 2.3215" xyz="0.01625 0.01516 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_middle_joint_median_actuating_hinge_median_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paradistal hinge -->
  <joint name="l_finger_middle_joint_paradistal_hinge" type="revolute">
    <parent link="l_finger_middle_link_2"/>
    <child link="l_finger_middle_link_paradistal_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0.039 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_middle_joint_paradistal_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paramedian hinge median bar underactuated -->
  <joint name="l_finger_middle_joint_paramedian_hinge_median_bar_underactuated" type="revolute">
    <parent link="l_finger_middle_link_paramedian_hinge"/>
    <child link="l_finger_middle_link_median_bar_underactuated"/>
    <axis xyz="0 0 1"/>
    <!-- hand calc 90 deg + atan(0.01227/0.00329) -->
    <origin rpy="0 0 1.5708" xyz="0.01227 0.00329 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="l_finger_middle_joint_paramedian_hinge_median_bar_underactuated">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 1 -->
  <joint name="l_finger_middle_joint_1" type="revolute">
    <origin rpy="0 0 -2.0906" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <parent link="l_finger_middle_link_0"/>
    <child link="l_finger_middle_link_1"/>
    <dynamics damping="1" friction="0"/>
    <!-- Limits: [0deg.,70deg.] -->
    <limit effort="60" lower="0" upper="1.2217" velocity="-1"/>
  </joint>
  <gazebo reference="l_finger_middle_joint_1">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 2 -->
  <joint name="l_finger_middle_joint_2" type="revolute">
    <parent link="l_finger_middle_link_1"/>
    <child link="l_finger_middle_link_2"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0.05715 0"/>
    <!-- Limits: [0deg.,90deg.] -->
    <limit effort="60" lower="0" upper="1.5708" velocity="-1"/>
  </joint>
  <gazebo reference="l_finger_middle_joint_2">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 3 -->
  <joint name="l_finger_middle_joint_3" type="revolute">
    <parent link="l_finger_middle_link_2"/>
    <child link="l_finger_middle_link_3"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 2.0906" xyz="0 0.039 0"/>
    <!-- Limits: [-38deg.,60deg.] -->
    <limit effort="60" lower="-0.6632" upper="1.0471" velocity="-1"/>
  </joint>
  <gazebo reference="l_finger_middle_joint_3">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- end of joint list -->
  <!-- coupling joints in sdf -->
  <gazebo>
    <joint name="l_finger_middle_link_proximal_actuating_bar_median_actuating_hinge_couple" type="revolute">
      <parent>l_finger_middle_link_median_actuating_hinge</parent>
      <child>l_finger_middle_link_proximal_actuating_bar</child>
      <pose>0.05398 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_middle_link_proximal_actuating_bar_median_actuating_hinge_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="l_finger_middle_link_median_bar_link_3_couple" type="revolute">
      <parent>l_finger_middle_link_3</parent>
      <child>l_finger_middle_link_median_bar</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_middle_link_median_bar_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="l_finger_middle_link_paramedian_bar_paradistal_hinge_couple" type="revolute">
      <parent>l_finger_middle_link_paradistal_hinge</parent>
      <child>l_finger_middle_link_median_bar_underactuated</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_middle_link_median_bar_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="l_finger_middle_link_paradistal_hinge_link_3_couple" type="revolute">
      <parent>l_finger_middle_link_paradistal_hinge</parent>
      <child>l_finger_middle_link_3</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_middle_link_paradistal_hinge_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="l_finger_middle_link_paraproximal_bar_paramedian_hinge_couple" type="revolute">
      <parent>l_finger_middle_link_paramedian_hinge</parent>
      <child>l_finger_middle_link_paraproximal_bar</child>
      <pose>0.05770 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_finger_middle_link_paraproximal_bar_paramedian_hinge_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- link list -->
  <!-- Link palm -->
  <link name="l_palm">
    <visual>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/palm.STL"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
    </visual>
    <!-- Simple shape collision -->
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="1.3"/>
      <inertia ixx="0.0035" ixy="0" ixz="-0" iyy="0.0035" iyz="0" izz="0.0035"/>
    </inertial>
  </link>
  <gazebo reference="l_palm">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- End of link list -->
  <!-- Joint list -->
  <!-- Joint palm_finger_1_joint -->
  <joint name="l_palm_finger_1_joint" type="revolute">
    <parent link="l_palm"/>
    <child link="l_finger_1_link_0"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 3.1416 -1.57" xyz="-0.0455 0.0214 0.036"/>
    <!-- Limits: [-17deg.,17deg.] -->
    <limit effort="60" lower="-0.2967" upper="0.2967" velocity="-1"/>
  </joint>
  <gazebo reference="l_palm_finger_1_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint palm_finger_2_joint -->
  <joint name="l_palm_finger_2_joint" type="revolute">
    <parent link="l_palm"/>
    <child link="l_finger_2_link_0"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 3.1416 -1.57" xyz="-0.0455 0.0214 -0.036"/>
    <!-- Limits: [-17deg.,17deg.] -->
    <limit effort="60" lower="-0.2967" upper="0.2967" velocity="-1"/>
  </joint>
  <gazebo reference="l_palm_finger_2_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint palm_finger_middle_joint -->
  <joint name="l_palm_finger_middle_joint" type="revolute">
    <parent link="l_palm"/>
    <child link="l_finger_middle_link_0"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 1.57" xyz="0.0455 0.0214 0"/>
    <limit effort="60" lower="0" upper="0" velocity="-1"/>
  </joint>
  <gazebo reference="l_palm_finger_middle_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- end of joint list -->
  <!-- joint attaching parent link to robotiq hand -->
  <joint name="r_robotiq_hand_joint" type="fixed">
    <origin rpy="1.57 0 2.355" xyz="0 0 0.05398"/>
    <parent link="r_robotiq_fts300_fts_toolside"/>
    <child link="r_palm"/>
  </joint>
  <!-- Insert the fingers -->
  <!-- Link list -->
  <!-- Link 0 -->
  <link name="r_finger_1_link_0">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_0.STL"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_0.STL"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <origin xyz="-0.01 0 0"/>
      <inertia ixx="0.000018" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_1_link_0">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 1 -->
  <link name="r_finger_1_link_1">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0.05717 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_1.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 1.5708" xyz="0 0.05717 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_1.STL"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <origin rpy="0 0 1.5708" xyz="0.00053 0.03218 0"/>
      <mass value="0.047"/>
      <inertia ixx="0.00000499332" ixy="0.00000091312" ixz="-0.00000000120" iyy="0.00006434531" iyz="-0.00000000007" izz="0.00006061917"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_1_link_1">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 2 -->
  <link name="r_finger_1_link_2">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0.03810 0.0075"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_2.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 1.5708" xyz="0 0.03810 0.0075"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_2.STL"/>
      </geometry>
      <material name="yellow"/>
    </collision>
    <inertial>
      <origin rpy="0 0 1.5708" xyz="0.00048 0.02167 0.00018"/>
      <mass value="0.03284"/>
      <inertia ixx="0.00000348008" ixy="0.00000031940" ixz="0.00000013041" iyy="0.00002248123" iyz="0.00000000291" izz="0.00001986346"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_1_link_2">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 3 -->
  <link name="r_finger_1_link_3">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_3.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_3.STL"/>
      </geometry>
      <material name="yellow"/>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01626 0.00049 0.0002"/>
      <mass value="0.03354"/>
      <inertia ixx="0.00000305151" ixy="0.00000089792" ixz="0.00000000249" iyy="0.00001427978" iyz="-0.00000001191" izz="0.00001264126"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_1_link_3">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link proximal actuating hinge -->
  <link name="r_finger_1_link_proximal_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0.01429 0 0"/>
      <geometry>
        <box size="0.02858 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01429 0 0"/>
      <geometry>
        <box size="0.02858 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0012 -0.00162 -0.00161"/>
      <mass value="0.02647"/>
      <inertia ixx="0.00000156447" ixy="0.000000236226" ixz="-0.00000014862" iyy="0.00000221068" iyz="-0.00000014369" izz="0.00000323175"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_1_link_proximal_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paraproximal actuating hinge -->
  <link name="r_finger_1_link_paraproximal_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0.01143 0 0"/>
      <geometry>
        <box size="0.02286 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01143 0 0"/>
      <geometry>
        <box size="0.02286 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0064 0 0"/>
      <!-- copied from proximal_actuating_hinge -->
      <mass value="0.00362"/>
      <inertia ixx="0.00000012043" ixy="0" ixz="0" iyy="0.00000053588" iyz="0" izz="0.00000065322"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_1_link_paraproximal_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link proximal actuating bar -->
  <link name="r_finger_1_link_proximal_actuating_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.026789 0 0"/>
      <geometry>
        <box size="0.05398 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.026789 0 0"/>
      <geometry>
        <box size="0.05398 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.02699 -0.00122 0"/>
      <mass value="0.00659"/>
      <inertia ixx="0.00000008348" ixy="-0.0000002179" ixz="0.00000000134" iyy="0.00000719076" iyz="0" izz="0.00000718272"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_1_link_proximal_actuating_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paraproximal bar -->
  <link name="r_finger_1_link_paraproximal_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.02885 0 0"/>
      <geometry>
        <box size="0.05770 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.02885 0 0"/>
      <geometry>
        <box size="0.05770 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.02858 -0.00129 0"/>
      <mass value="0.00732"/>
      <inertia ixx="0.00000009065" ixy="-0.00000027047" ixz="0.00000000142" iyy="0.00000878034" iyz="0" izz="0.00000877558"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_1_link_paraproximal_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median actuating hinge -->
  <link name="r_finger_1_link_median_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0092 0.00514 0"/>
      <mass value="0.00359"/>
      <inertia ixx="0.00000024735" ixy="0.00000023619" ixz="0" iyy="0.00000047681" iyz="0" izz="0.00000071813"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_1_link_median_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median bar -->
  <link name="r_finger_1_link_median_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01905 -0.00107 0"/>
      <mass value="0.00538"/>
      <inertia ixx="0.00000006639" ixy="-0.00000010996" ixz="0.00000000094" iyy="0.00000305318" iyz="0" izz="0.00000304657"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_1_link_median_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paramedian hinge -->
  <link name="r_finger_1_link_paramedian_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01025 0.00046 0"/>
      <mass value="0.00276"/>
      <inertia ixx="0.00000004213" ixy="-0.00000001491" ixz="0" iyy="0.00000050258" iyz="0" izz="0.00000054007"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_1_link_paramedian_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median bar underactuated -->
  <link name="r_finger_1_link_median_bar_underactuated">
    <visual>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01905 -0.00107 0"/>
      <mass value="0.00538"/>
      <inertia ixx="0.00000006639" ixy="-0.00000010996" ixz="-0.00000000094" iyy="0.00000305318" iyz="0" izz="0.00000304657"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_1_link_median_bar_underactuated">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paradistal hinge -->
  <link name="r_finger_1_link_paradistal_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 0.02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.00548 0 0"/>
      <mass value="0.00158"/>
      <inertia ixx="0.00000001907" ixy="0" ixz="0" iyy="0.00000010970" iyz="0" izz="0.00000012610"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_1_link_paradistal_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- End of link list -->
  <!-- Joint list -->
  <!-- Joint proximal actuating hinge -->
  <joint name="r_finger_1_joint_proximal_actuating_hinge" type="revolute">
    <parent link="r_finger_1_link_0"/>
    <child link="r_finger_1_link_proximal_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -2.0906" xyz="0 0 0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_1_joint_proximal_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paraproximal actuating hinge -->
  <joint name="r_finger_1_joint_paraproximal_actuating_hinge" type="revolute">
    <parent link="r_finger_1_link_0"/>
    <child link="r_finger_1_link_paraproximal_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -2.0906" xyz="0 0 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_1_joint_paraproximal_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint proximal actuating bar -->
  <joint name="r_finger_1_joint_proximal_actuating_bar" type="revolute">
    <parent link="r_finger_1_link_proximal_actuating_hinge"/>
    <child link="r_finger_1_link_proximal_actuating_bar"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 1.8616" xyz="0.02858 0 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_1_joint_proximal_actuating_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paraproximal bar -->
  <joint name="r_finger_1_joint_paraproximal_bar" type="revolute">
    <parent link="r_finger_1_link_paraproximal_actuating_hinge"/>
    <child link="r_finger_1_link_paraproximal_bar"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 1.57" xyz="0.02286 0 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_1_joint_paraproximal_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint median actuating hinge -->
  <joint name="r_finger_1_joint_median_actuating_hinge" type="revolute">
    <parent link="r_finger_1_link_1"/>
    <child link="r_finger_1_link_median_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -0.37" xyz="0 0.05715 0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_1_joint_median_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paramedian hinge -->
  <joint name="r_finger_1_joint_paramedian_hinge" type="revolute">
    <parent link="r_finger_1_link_1"/>
    <child link="r_finger_1_link_paramedian_hinge"/>
    <axis xyz="0 0 1"/>
    <!-- -0.37 is hand picked, 0.9719 is hand calculated from atan(0.02381/0.01625) -->
    <origin rpy="0 0 0" xyz="0 0.05715 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_1_joint_paramedian_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint median actuating hinge median bar -->
  <joint name="r_finger_1_joint_median_actuating_hinge_median_bar" type="revolute">
    <parent link="r_finger_1_link_median_actuating_hinge"/>
    <child link="r_finger_1_link_median_bar"/>
    <axis xyz="0 0 1"/>
    <!-- hand calc 90 deg + atan(0.01516/0.01625) -->
    <origin rpy="0 0 2.3215" xyz="0.01625 0.01516 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_1_joint_median_actuating_hinge_median_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paradistal hinge -->
  <joint name="r_finger_1_joint_paradistal_hinge" type="revolute">
    <parent link="r_finger_1_link_2"/>
    <child link="r_finger_1_link_paradistal_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0.039 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_1_joint_paradistal_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paramedian hinge median bar underactuated -->
  <joint name="r_finger_1_joint_paramedian_hinge_median_bar_underactuated" type="revolute">
    <parent link="r_finger_1_link_paramedian_hinge"/>
    <child link="r_finger_1_link_median_bar_underactuated"/>
    <axis xyz="0 0 1"/>
    <!-- hand calc 90 deg + atan(0.01227/0.00329) -->
    <origin rpy="0 0 1.5708" xyz="0.01227 0.00329 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_1_joint_paramedian_hinge_median_bar_underactuated">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 1 -->
  <joint name="r_finger_1_joint_1" type="revolute">
    <origin rpy="0 0 -2.0906" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <parent link="r_finger_1_link_0"/>
    <child link="r_finger_1_link_1"/>
    <dynamics damping="1" friction="0"/>
    <!-- Limits: [0deg.,70deg.] -->
    <limit effort="60" lower="0" upper="1.2217" velocity="-1"/>
  </joint>
  <gazebo reference="r_finger_1_joint_1">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 2 -->
  <joint name="r_finger_1_joint_2" type="revolute">
    <parent link="r_finger_1_link_1"/>
    <child link="r_finger_1_link_2"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0.05715 0"/>
    <!-- Limits: [0deg.,90deg.] -->
    <limit effort="60" lower="0" upper="1.5708" velocity="-1"/>
  </joint>
  <gazebo reference="r_finger_1_joint_2">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 3 -->
  <joint name="r_finger_1_joint_3" type="revolute">
    <parent link="r_finger_1_link_2"/>
    <child link="r_finger_1_link_3"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 2.0906" xyz="0 0.039 0"/>
    <!-- Limits: [-38deg.,60deg.] -->
    <limit effort="60" lower="-0.6632" upper="1.0471" velocity="-1"/>
  </joint>
  <gazebo reference="r_finger_1_joint_3">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- end of joint list -->
  <!-- coupling joints in sdf -->
  <gazebo>
    <joint name="r_finger_1_link_proximal_actuating_bar_median_actuating_hinge_couple" type="revolute">
      <parent>r_finger_1_link_median_actuating_hinge</parent>
      <child>r_finger_1_link_proximal_actuating_bar</child>
      <pose>0.05398 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_1_link_proximal_actuating_bar_median_actuating_hinge_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="r_finger_1_link_median_bar_link_3_couple" type="revolute">
      <parent>r_finger_1_link_3</parent>
      <child>r_finger_1_link_median_bar</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_1_link_median_bar_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="r_finger_1_link_paramedian_bar_paradistal_hinge_couple" type="revolute">
      <parent>r_finger_1_link_paradistal_hinge</parent>
      <child>r_finger_1_link_median_bar_underactuated</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_1_link_median_bar_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="r_finger_1_link_paradistal_hinge_link_3_couple" type="revolute">
      <parent>r_finger_1_link_paradistal_hinge</parent>
      <child>r_finger_1_link_3</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_1_link_paradistal_hinge_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="r_finger_1_link_paraproximal_bar_paramedian_hinge_couple" type="revolute">
      <parent>r_finger_1_link_paramedian_hinge</parent>
      <child>r_finger_1_link_paraproximal_bar</child>
      <pose>0.05770 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_1_link_paraproximal_bar_paramedian_hinge_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Link list -->
  <!-- Link 0 -->
  <link name="r_finger_2_link_0">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_0.STL"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_0.STL"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <origin xyz="-0.01 0 0"/>
      <inertia ixx="0.000018" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_2_link_0">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 1 -->
  <link name="r_finger_2_link_1">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0.05717 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_1.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 1.5708" xyz="0 0.05717 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_1.STL"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <origin rpy="0 0 1.5708" xyz="0.00053 0.03218 0"/>
      <mass value="0.047"/>
      <inertia ixx="0.00000499332" ixy="0.00000091312" ixz="-0.00000000120" iyy="0.00006434531" iyz="-0.00000000007" izz="0.00006061917"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_2_link_1">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 2 -->
  <link name="r_finger_2_link_2">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0.03810 0.0075"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_2.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 1.5708" xyz="0 0.03810 0.0075"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_2.STL"/>
      </geometry>
      <material name="yellow"/>
    </collision>
    <inertial>
      <origin rpy="0 0 1.5708" xyz="0.00048 0.02167 0.00018"/>
      <mass value="0.03284"/>
      <inertia ixx="0.00000348008" ixy="0.00000031940" ixz="0.00000013041" iyy="0.00002248123" iyz="0.00000000291" izz="0.00001986346"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_2_link_2">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 3 -->
  <link name="r_finger_2_link_3">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_3.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_3.STL"/>
      </geometry>
      <material name="yellow"/>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01626 0.00049 0.0002"/>
      <mass value="0.03354"/>
      <inertia ixx="0.00000305151" ixy="0.00000089792" ixz="0.00000000249" iyy="0.00001427978" iyz="-0.00000001191" izz="0.00001264126"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_2_link_3">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link proximal actuating hinge -->
  <link name="r_finger_2_link_proximal_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0.01429 0 0"/>
      <geometry>
        <box size="0.02858 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01429 0 0"/>
      <geometry>
        <box size="0.02858 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0012 -0.00162 -0.00161"/>
      <mass value="0.02647"/>
      <inertia ixx="0.00000156447" ixy="0.000000236226" ixz="-0.00000014862" iyy="0.00000221068" iyz="-0.00000014369" izz="0.00000323175"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_2_link_proximal_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paraproximal actuating hinge -->
  <link name="r_finger_2_link_paraproximal_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0.01143 0 0"/>
      <geometry>
        <box size="0.02286 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01143 0 0"/>
      <geometry>
        <box size="0.02286 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0064 0 0"/>
      <!-- copied from proximal_actuating_hinge -->
      <mass value="0.00362"/>
      <inertia ixx="0.00000012043" ixy="0" ixz="0" iyy="0.00000053588" iyz="0" izz="0.00000065322"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_2_link_paraproximal_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link proximal actuating bar -->
  <link name="r_finger_2_link_proximal_actuating_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.026789 0 0"/>
      <geometry>
        <box size="0.05398 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.026789 0 0"/>
      <geometry>
        <box size="0.05398 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.02699 -0.00122 0"/>
      <mass value="0.00659"/>
      <inertia ixx="0.00000008348" ixy="-0.0000002179" ixz="0.00000000134" iyy="0.00000719076" iyz="0" izz="0.00000718272"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_2_link_proximal_actuating_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paraproximal bar -->
  <link name="r_finger_2_link_paraproximal_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.02885 0 0"/>
      <geometry>
        <box size="0.05770 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.02885 0 0"/>
      <geometry>
        <box size="0.05770 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.02858 -0.00129 0"/>
      <mass value="0.00732"/>
      <inertia ixx="0.00000009065" ixy="-0.00000027047" ixz="0.00000000142" iyy="0.00000878034" iyz="0" izz="0.00000877558"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_2_link_paraproximal_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median actuating hinge -->
  <link name="r_finger_2_link_median_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0092 0.00514 0"/>
      <mass value="0.00359"/>
      <inertia ixx="0.00000024735" ixy="0.00000023619" ixz="0" iyy="0.00000047681" iyz="0" izz="0.00000071813"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_2_link_median_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median bar -->
  <link name="r_finger_2_link_median_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01905 -0.00107 0"/>
      <mass value="0.00538"/>
      <inertia ixx="0.00000006639" ixy="-0.00000010996" ixz="0.00000000094" iyy="0.00000305318" iyz="0" izz="0.00000304657"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_2_link_median_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paramedian hinge -->
  <link name="r_finger_2_link_paramedian_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01025 0.00046 0"/>
      <mass value="0.00276"/>
      <inertia ixx="0.00000004213" ixy="-0.00000001491" ixz="0" iyy="0.00000050258" iyz="0" izz="0.00000054007"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_2_link_paramedian_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median bar underactuated -->
  <link name="r_finger_2_link_median_bar_underactuated">
    <visual>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01905 -0.00107 0"/>
      <mass value="0.00538"/>
      <inertia ixx="0.00000006639" ixy="-0.00000010996" ixz="-0.00000000094" iyy="0.00000305318" iyz="0" izz="0.00000304657"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_2_link_median_bar_underactuated">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paradistal hinge -->
  <link name="r_finger_2_link_paradistal_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 0.02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.00548 0 0"/>
      <mass value="0.00158"/>
      <inertia ixx="0.00000001907" ixy="0" ixz="0" iyy="0.00000010970" iyz="0" izz="0.00000012610"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_2_link_paradistal_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- End of link list -->
  <!-- Joint list -->
  <!-- Joint proximal actuating hinge -->
  <joint name="r_finger_2_joint_proximal_actuating_hinge" type="revolute">
    <parent link="r_finger_2_link_0"/>
    <child link="r_finger_2_link_proximal_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -2.0906" xyz="0 0 0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_2_joint_proximal_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paraproximal actuating hinge -->
  <joint name="r_finger_2_joint_paraproximal_actuating_hinge" type="revolute">
    <parent link="r_finger_2_link_0"/>
    <child link="r_finger_2_link_paraproximal_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -2.0906" xyz="0 0 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_2_joint_paraproximal_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint proximal actuating bar -->
  <joint name="r_finger_2_joint_proximal_actuating_bar" type="revolute">
    <parent link="r_finger_2_link_proximal_actuating_hinge"/>
    <child link="r_finger_2_link_proximal_actuating_bar"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 1.8616" xyz="0.02858 0 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_2_joint_proximal_actuating_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paraproximal bar -->
  <joint name="r_finger_2_joint_paraproximal_bar" type="revolute">
    <parent link="r_finger_2_link_paraproximal_actuating_hinge"/>
    <child link="r_finger_2_link_paraproximal_bar"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 1.57" xyz="0.02286 0 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_2_joint_paraproximal_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint median actuating hinge -->
  <joint name="r_finger_2_joint_median_actuating_hinge" type="revolute">
    <parent link="r_finger_2_link_1"/>
    <child link="r_finger_2_link_median_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -0.37" xyz="0 0.05715 0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_2_joint_median_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paramedian hinge -->
  <joint name="r_finger_2_joint_paramedian_hinge" type="revolute">
    <parent link="r_finger_2_link_1"/>
    <child link="r_finger_2_link_paramedian_hinge"/>
    <axis xyz="0 0 1"/>
    <!-- -0.37 is hand picked, 0.9719 is hand calculated from atan(0.02381/0.01625) -->
    <origin rpy="0 0 0" xyz="0 0.05715 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_2_joint_paramedian_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint median actuating hinge median bar -->
  <joint name="r_finger_2_joint_median_actuating_hinge_median_bar" type="revolute">
    <parent link="r_finger_2_link_median_actuating_hinge"/>
    <child link="r_finger_2_link_median_bar"/>
    <axis xyz="0 0 1"/>
    <!-- hand calc 90 deg + atan(0.01516/0.01625) -->
    <origin rpy="0 0 2.3215" xyz="0.01625 0.01516 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_2_joint_median_actuating_hinge_median_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paradistal hinge -->
  <joint name="r_finger_2_joint_paradistal_hinge" type="revolute">
    <parent link="r_finger_2_link_2"/>
    <child link="r_finger_2_link_paradistal_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0.039 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_2_joint_paradistal_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paramedian hinge median bar underactuated -->
  <joint name="r_finger_2_joint_paramedian_hinge_median_bar_underactuated" type="revolute">
    <parent link="r_finger_2_link_paramedian_hinge"/>
    <child link="r_finger_2_link_median_bar_underactuated"/>
    <axis xyz="0 0 1"/>
    <!-- hand calc 90 deg + atan(0.01227/0.00329) -->
    <origin rpy="0 0 1.5708" xyz="0.01227 0.00329 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_2_joint_paramedian_hinge_median_bar_underactuated">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 1 -->
  <joint name="r_finger_2_joint_1" type="revolute">
    <origin rpy="0 0 -2.0906" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <parent link="r_finger_2_link_0"/>
    <child link="r_finger_2_link_1"/>
    <dynamics damping="1" friction="0"/>
    <!-- Limits: [0deg.,70deg.] -->
    <limit effort="60" lower="0" upper="1.2217" velocity="-1"/>
  </joint>
  <gazebo reference="r_finger_2_joint_1">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 2 -->
  <joint name="r_finger_2_joint_2" type="revolute">
    <parent link="r_finger_2_link_1"/>
    <child link="r_finger_2_link_2"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0.05715 0"/>
    <!-- Limits: [0deg.,90deg.] -->
    <limit effort="60" lower="0" upper="1.5708" velocity="-1"/>
  </joint>
  <gazebo reference="r_finger_2_joint_2">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 3 -->
  <joint name="r_finger_2_joint_3" type="revolute">
    <parent link="r_finger_2_link_2"/>
    <child link="r_finger_2_link_3"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 2.0906" xyz="0 0.039 0"/>
    <!-- Limits: [-38deg.,60deg.] -->
    <limit effort="60" lower="-0.6632" upper="1.0471" velocity="-1"/>
  </joint>
  <gazebo reference="r_finger_2_joint_3">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- end of joint list -->
  <!-- coupling joints in sdf -->
  <gazebo>
    <joint name="r_finger_2_link_proximal_actuating_bar_median_actuating_hinge_couple" type="revolute">
      <parent>r_finger_2_link_median_actuating_hinge</parent>
      <child>r_finger_2_link_proximal_actuating_bar</child>
      <pose>0.05398 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_2_link_proximal_actuating_bar_median_actuating_hinge_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="r_finger_2_link_median_bar_link_3_couple" type="revolute">
      <parent>r_finger_2_link_3</parent>
      <child>r_finger_2_link_median_bar</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_2_link_median_bar_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="r_finger_2_link_paramedian_bar_paradistal_hinge_couple" type="revolute">
      <parent>r_finger_2_link_paradistal_hinge</parent>
      <child>r_finger_2_link_median_bar_underactuated</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_2_link_median_bar_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="r_finger_2_link_paradistal_hinge_link_3_couple" type="revolute">
      <parent>r_finger_2_link_paradistal_hinge</parent>
      <child>r_finger_2_link_3</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_2_link_paradistal_hinge_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="r_finger_2_link_paraproximal_bar_paramedian_hinge_couple" type="revolute">
      <parent>r_finger_2_link_paramedian_hinge</parent>
      <child>r_finger_2_link_paraproximal_bar</child>
      <pose>0.05770 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_2_link_paraproximal_bar_paramedian_hinge_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Link list -->
  <!-- Link 0 -->
  <link name="r_finger_middle_link_0">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_0.STL"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_0.STL"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <origin xyz="-0.01 0 0"/>
      <inertia ixx="0.000018" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_middle_link_0">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 1 -->
  <link name="r_finger_middle_link_1">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0.05717 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_1.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 1.5708" xyz="0 0.05717 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_1.STL"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <origin rpy="0 0 1.5708" xyz="0.00053 0.03218 0"/>
      <mass value="0.047"/>
      <inertia ixx="0.00000499332" ixy="0.00000091312" ixz="-0.00000000120" iyy="0.00006434531" iyz="-0.00000000007" izz="0.00006061917"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_middle_link_1">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 2 -->
  <link name="r_finger_middle_link_2">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0.03810 0.0075"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_2.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 1.5708" xyz="0 0.03810 0.0075"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_2.STL"/>
      </geometry>
      <material name="yellow"/>
    </collision>
    <inertial>
      <origin rpy="0 0 1.5708" xyz="0.00048 0.02167 0.00018"/>
      <mass value="0.03284"/>
      <inertia ixx="0.00000348008" ixy="0.00000031940" ixz="0.00000013041" iyy="0.00002248123" iyz="0.00000000291" izz="0.00001986346"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_middle_link_2">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link 3 -->
  <link name="r_finger_middle_link_3">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/link_3.STL"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/collision/link_3.STL"/>
      </geometry>
      <material name="yellow"/>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01626 0.00049 0.0002"/>
      <mass value="0.03354"/>
      <inertia ixx="0.00000305151" ixy="0.00000089792" ixz="0.00000000249" iyy="0.00001427978" iyz="-0.00000001191" izz="0.00001264126"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_middle_link_3">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link proximal actuating hinge -->
  <link name="r_finger_middle_link_proximal_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0.01429 0 0"/>
      <geometry>
        <box size="0.02858 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01429 0 0"/>
      <geometry>
        <box size="0.02858 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0012 -0.00162 -0.00161"/>
      <mass value="0.02647"/>
      <inertia ixx="0.00000156447" ixy="0.000000236226" ixz="-0.00000014862" iyy="0.00000221068" iyz="-0.00000014369" izz="0.00000323175"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_middle_link_proximal_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paraproximal actuating hinge -->
  <link name="r_finger_middle_link_paraproximal_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0.01143 0 0"/>
      <geometry>
        <box size="0.02286 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01143 0 0"/>
      <geometry>
        <box size="0.02286 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0064 0 0"/>
      <!-- copied from proximal_actuating_hinge -->
      <mass value="0.00362"/>
      <inertia ixx="0.00000012043" ixy="0" ixz="0" iyy="0.00000053588" iyz="0" izz="0.00000065322"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_middle_link_paraproximal_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link proximal actuating bar -->
  <link name="r_finger_middle_link_proximal_actuating_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.026789 0 0"/>
      <geometry>
        <box size="0.05398 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.026789 0 0"/>
      <geometry>
        <box size="0.05398 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.02699 -0.00122 0"/>
      <mass value="0.00659"/>
      <inertia ixx="0.00000008348" ixy="-0.0000002179" ixz="0.00000000134" iyy="0.00000719076" iyz="0" izz="0.00000718272"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_middle_link_proximal_actuating_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paraproximal bar -->
  <link name="r_finger_middle_link_paraproximal_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.02885 0 0"/>
      <geometry>
        <box size="0.05770 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.02885 0 0"/>
      <geometry>
        <box size="0.05770 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.02858 -0.00129 0"/>
      <mass value="0.00732"/>
      <inertia ixx="0.00000009065" ixy="-0.00000027047" ixz="0.00000000142" iyy="0.00000878034" iyz="0" izz="0.00000877558"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_middle_link_paraproximal_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median actuating hinge -->
  <link name="r_finger_middle_link_median_actuating_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0092 0.00514 0"/>
      <mass value="0.00359"/>
      <inertia ixx="0.00000024735" ixy="0.00000023619" ixz="0" iyy="0.00000047681" iyz="0" izz="0.00000071813"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_middle_link_median_actuating_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median bar -->
  <link name="r_finger_middle_link_median_bar">
    <visual>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01905 -0.00107 0"/>
      <mass value="0.00538"/>
      <inertia ixx="0.00000006639" ixy="-0.00000010996" ixz="0.00000000094" iyy="0.00000305318" iyz="0" izz="0.00000304657"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_middle_link_median_bar">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paramedian hinge -->
  <link name="r_finger_middle_link_paramedian_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01025 0.00046 0"/>
      <mass value="0.00276"/>
      <inertia ixx="0.00000004213" ixy="-0.00000001491" ixz="0" iyy="0.00000050258" iyz="0" izz="0.00000054007"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_middle_link_paramedian_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link median bar underactuated -->
  <link name="r_finger_middle_link_median_bar_underactuated">
    <visual>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.01905 0 0"/>
      <geometry>
        <box size="0.0381 .0075 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.01905 -0.00107 0"/>
      <mass value="0.00538"/>
      <inertia ixx="0.00000006639" ixy="-0.00000010996" ixz="-0.00000000094" iyy="0.00000305318" iyz="0" izz="0.00000304657"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_middle_link_median_bar_underactuated">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- Link paradistal hinge -->
  <link name="r_finger_middle_link_paradistal_hinge">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 .02 .002"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".02 0.02 .002"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.00548 0 0"/>
      <mass value="0.00158"/>
      <inertia ixx="0.00000001907" ixy="0" ixz="0" iyy="0.00000010970" iyz="0" izz="0.00000012610"/>
    </inertial>
  </link>
  <gazebo reference="r_finger_middle_link_paradistal_hinge">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- End of link list -->
  <!-- Joint list -->
  <!-- Joint proximal actuating hinge -->
  <joint name="r_finger_middle_joint_proximal_actuating_hinge" type="revolute">
    <parent link="r_finger_middle_link_0"/>
    <child link="r_finger_middle_link_proximal_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -2.0906" xyz="0 0 0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_middle_joint_proximal_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paraproximal actuating hinge -->
  <joint name="r_finger_middle_joint_paraproximal_actuating_hinge" type="revolute">
    <parent link="r_finger_middle_link_0"/>
    <child link="r_finger_middle_link_paraproximal_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -2.0906" xyz="0 0 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_middle_joint_paraproximal_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint proximal actuating bar -->
  <joint name="r_finger_middle_joint_proximal_actuating_bar" type="revolute">
    <parent link="r_finger_middle_link_proximal_actuating_hinge"/>
    <child link="r_finger_middle_link_proximal_actuating_bar"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 1.8616" xyz="0.02858 0 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_middle_joint_proximal_actuating_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paraproximal bar -->
  <joint name="r_finger_middle_joint_paraproximal_bar" type="revolute">
    <parent link="r_finger_middle_link_paraproximal_actuating_hinge"/>
    <child link="r_finger_middle_link_paraproximal_bar"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 1.57" xyz="0.02286 0 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_middle_joint_paraproximal_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint median actuating hinge -->
  <joint name="r_finger_middle_joint_median_actuating_hinge" type="revolute">
    <parent link="r_finger_middle_link_1"/>
    <child link="r_finger_middle_link_median_actuating_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 -0.37" xyz="0 0.05715 0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_middle_joint_median_actuating_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paramedian hinge -->
  <joint name="r_finger_middle_joint_paramedian_hinge" type="revolute">
    <parent link="r_finger_middle_link_1"/>
    <child link="r_finger_middle_link_paramedian_hinge"/>
    <axis xyz="0 0 1"/>
    <!-- -0.37 is hand picked, 0.9719 is hand calculated from atan(0.02381/0.01625) -->
    <origin rpy="0 0 0" xyz="0 0.05715 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_middle_joint_paramedian_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint median actuating hinge median bar -->
  <joint name="r_finger_middle_joint_median_actuating_hinge_median_bar" type="revolute">
    <parent link="r_finger_middle_link_median_actuating_hinge"/>
    <child link="r_finger_middle_link_median_bar"/>
    <axis xyz="0 0 1"/>
    <!-- hand calc 90 deg + atan(0.01516/0.01625) -->
    <origin rpy="0 0 2.3215" xyz="0.01625 0.01516 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_middle_joint_median_actuating_hinge_median_bar">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paradistal hinge -->
  <joint name="r_finger_middle_joint_paradistal_hinge" type="revolute">
    <parent link="r_finger_middle_link_2"/>
    <child link="r_finger_middle_link_paradistal_hinge"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0.039 -0.01"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_middle_joint_paradistal_hinge">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint paramedian hinge median bar underactuated -->
  <joint name="r_finger_middle_joint_paramedian_hinge_median_bar_underactuated" type="revolute">
    <parent link="r_finger_middle_link_paramedian_hinge"/>
    <child link="r_finger_middle_link_median_bar_underactuated"/>
    <axis xyz="0 0 1"/>
    <!-- hand calc 90 deg + atan(0.01227/0.00329) -->
    <origin rpy="0 0 1.5708" xyz="0.01227 0.00329 0"/>
    <limit effort="100" lower="-3.1416" upper="3.1416" velocity="100"/>
  </joint>
  <gazebo reference="r_finger_middle_joint_paramedian_hinge_median_bar_underactuated">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 1 -->
  <joint name="r_finger_middle_joint_1" type="revolute">
    <origin rpy="0 0 -2.0906" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <parent link="r_finger_middle_link_0"/>
    <child link="r_finger_middle_link_1"/>
    <dynamics damping="1" friction="0"/>
    <!-- Limits: [0deg.,70deg.] -->
    <limit effort="60" lower="0" upper="1.2217" velocity="-1"/>
  </joint>
  <gazebo reference="r_finger_middle_joint_1">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 2 -->
  <joint name="r_finger_middle_joint_2" type="revolute">
    <parent link="r_finger_middle_link_1"/>
    <child link="r_finger_middle_link_2"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0.05715 0"/>
    <!-- Limits: [0deg.,90deg.] -->
    <limit effort="60" lower="0" upper="1.5708" velocity="-1"/>
  </joint>
  <gazebo reference="r_finger_middle_joint_2">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint 3 -->
  <joint name="r_finger_middle_joint_3" type="revolute">
    <parent link="r_finger_middle_link_2"/>
    <child link="r_finger_middle_link_3"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 2.0906" xyz="0 0.039 0"/>
    <!-- Limits: [-38deg.,60deg.] -->
    <limit effort="60" lower="-0.6632" upper="1.0471" velocity="-1"/>
  </joint>
  <gazebo reference="r_finger_middle_joint_3">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- end of joint list -->
  <!-- coupling joints in sdf -->
  <gazebo>
    <joint name="r_finger_middle_link_proximal_actuating_bar_median_actuating_hinge_couple" type="revolute">
      <parent>r_finger_middle_link_median_actuating_hinge</parent>
      <child>r_finger_middle_link_proximal_actuating_bar</child>
      <pose>0.05398 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_middle_link_proximal_actuating_bar_median_actuating_hinge_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="r_finger_middle_link_median_bar_link_3_couple" type="revolute">
      <parent>r_finger_middle_link_3</parent>
      <child>r_finger_middle_link_median_bar</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_middle_link_median_bar_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="r_finger_middle_link_paramedian_bar_paradistal_hinge_couple" type="revolute">
      <parent>r_finger_middle_link_paradistal_hinge</parent>
      <child>r_finger_middle_link_median_bar_underactuated</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_middle_link_median_bar_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="r_finger_middle_link_paradistal_hinge_link_3_couple" type="revolute">
      <parent>r_finger_middle_link_paradistal_hinge</parent>
      <child>r_finger_middle_link_3</child>
      <pose>0.0381 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_middle_link_paradistal_hinge_link_3_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <joint name="r_finger_middle_link_paraproximal_bar_paramedian_hinge_couple" type="revolute">
      <parent>r_finger_middle_link_paramedian_hinge</parent>
      <child>r_finger_middle_link_paraproximal_bar</child>
      <pose>0.05770 0 0 0 0 0</pose>
      <axis>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
        <xyz>0 0 1</xyz>
        <!-- <use_parent_model_frame>false</use_parent_model_frame> -->
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_finger_middle_link_paraproximal_bar_paramedian_hinge_couple">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- link list -->
  <!-- Link palm -->
  <link name="r_palm">
    <visual>
      <geometry>
        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/palm.STL"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
    </visual>
    <!-- Simple shape collision -->
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
      <material name="yellow">
        <color rgba="0 1 1 1"/>
      </material>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="1.3"/>
      <inertia ixx="0.0035" ixy="0" ixz="-0" iyy="0.0035" iyz="0" izz="0.0035"/>
    </inertial>
  </link>
  <gazebo reference="r_palm">
    <mu1>1.1</mu1>
    <mu2>1.1</mu2>
  </gazebo>
  <!-- End of link list -->
  <!-- Joint list -->
  <!-- Joint palm_finger_1_joint -->
  <joint name="r_palm_finger_1_joint" type="revolute">
    <parent link="r_palm"/>
    <child link="r_finger_1_link_0"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 3.1416 -1.57" xyz="-0.0455 0.0214 0.036"/>
    <!-- Limits: [-17deg.,17deg.] -->
    <limit effort="60" lower="-0.2967" upper="0.2967" velocity="-1"/>
  </joint>
  <gazebo reference="r_palm_finger_1_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint palm_finger_2_joint -->
  <joint name="r_palm_finger_2_joint" type="revolute">
    <parent link="r_palm"/>
    <child link="r_finger_2_link_0"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 3.1416 -1.57" xyz="-0.0455 0.0214 -0.036"/>
    <!-- Limits: [-17deg.,17deg.] -->
    <limit effort="60" lower="-0.2967" upper="0.2967" velocity="-1"/>
  </joint>
  <gazebo reference="r_palm_finger_2_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Joint palm_finger_middle_joint -->
  <joint name="r_palm_finger_middle_joint" type="revolute">
    <parent link="r_palm"/>
    <child link="r_finger_middle_link_0"/>
    <dynamics damping="1" friction="0"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 1.57" xyz="0.0455 0.0214 0"/>
    <limit effort="60" lower="0" upper="0" velocity="-1"/>
  </joint>
  <gazebo reference="r_palm_finger_middle_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- end of joint list -->
  <!-- Join left arm to top plate -->
  <joint name="r_ur5_arm_base" type="fixed">
    <parent link="dual_arm_bulkhead_link"/>
    <child link="r_ur5_arm_base_link"/>
    <origin rpy="0 0.785 -1.57" xyz="0.125 -0.15 0.135"/>
  </joint>
  <!-- Joint right arm to top plate -->
  <joint name="l_ur5_arm_base" type="fixed">
    <parent link="dual_arm_bulkhead_link"/>
    <child link="l_ur5_arm_base_link"/>
    <origin rpy="0 -0.785 -1.57" xyz="0.125 0.15 0.135"/>
  </joint>
</robot>

