From 199e5c208fc768172683303b4f79799e22a93fa6 Mon Sep 17 00:00:00 2001
From: hongkai-dai <daihongkai@gmail.com>
Date: Fri, 13 Feb 2015 15:16:00 -0500
Subject: [PATCH] add IRB140 package in examples/IRB140 directory

---
 examples/IRB140/package.xml                |    0
 examples/IRB140/runPlanning.m              |    2 +-
 examples/IRB140/urdf/ATI_sensor.urdf       |    4 +-
 examples/IRB140/urdf/irb_140.urdf          |   28 +-
 examples/IRB140/urdf/irb_140_convhull.urdf |   28 +-
 examples/IRB140/urdf/irb_140_robotiq.urdf  | 2267 +++++++++++---------
 util/rospack.m                             |    2 +
 7 files changed, 1317 insertions(+), 1014 deletions(-)
 create mode 100644 examples/IRB140/package.xml

diff --git a/examples/IRB140/package.xml b/examples/IRB140/package.xml
new file mode 100644
index 000000000000..e69de29bb2d1
diff --git a/examples/IRB140/runPlanning.m b/examples/IRB140/runPlanning.m
index f85f9992677a..e6dd877140c5 100644
--- a/examples/IRB140/runPlanning.m
+++ b/examples/IRB140/runPlanning.m
@@ -22,7 +22,7 @@
 end
 
 r=RigidBodyManipulator();
-r = addRobotFromURDF(r,'irb_140.urdf',options.base_offset,options.base_rpy);
+r = addRobotFromURDF(r,'urdf/irb_140.urdf',options.base_offset,options.base_rpy);
 if (options.visualize)
   v=r.constructVisualizer();
 end
diff --git a/examples/IRB140/urdf/ATI_sensor.urdf b/examples/IRB140/urdf/ATI_sensor.urdf
index 27e4cdc3198d..d2c907fee035 100644
--- a/examples/IRB140/urdf/ATI_sensor.urdf
+++ b/examples/IRB140/urdf/ATI_sensor.urdf
@@ -22,7 +22,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="../meshes/ATI_sensor.STL" />
+          filename="package://IRB140/urdf/meshes/ATI_sensor.STL" />
       </geometry>
       <material
         name="">
@@ -36,7 +36,7 @@
         rpy="1.5707963267949 0 0" />
       <geometry>
         <mesh
-          filename="../meshes/ATI_sensor.STL" />
+          filename="package://IRB140/urdf/meshes/ATI_sensor.STL" />
       </geometry>
     </collision>
   </link>
diff --git a/examples/IRB140/urdf/irb_140.urdf b/examples/IRB140/urdf/irb_140.urdf
index fcfddeb8d4e0..985c39261caa 100644
--- a/examples/IRB140/urdf/irb_140.urdf
+++ b/examples/IRB140/urdf/irb_140.urdf
@@ -23,7 +23,7 @@
         rpy="0 0 0" />
 <!--<geometry>
         <mesh
-          filename="meshes/base_link.stl" />
+          filename="package://IRB140/urdf/meshes/base_link.stl" />
       </geometry>-->
       <material
         name="">
@@ -37,7 +37,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/base_link.stl" />
+          filename="package://IRB140/urdf/meshes/base_link.stl" />
       </geometry>
     </collision>
   </link>
@@ -63,7 +63,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link1.stl" />
+          filename="package://IRB140/urdf/meshes/link1.stl" />
       </geometry>
       <material
         name="">
@@ -77,7 +77,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link1.stl" />
+          filename="package://IRB140/urdf/meshes/link1.stl" />
       </geometry>
     </collision>
   </link>
@@ -103,7 +103,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link2.stl" />
+          filename="package://IRB140/urdf/meshes/link2.stl" />
       </geometry>
       <material
         name="">
@@ -117,7 +117,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link2.stl" />
+          filename="package://IRB140/urdf/meshes/link2.stl" />
       </geometry>
     </collision>
   </link>
@@ -143,7 +143,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link3.stl" />
+          filename="package://IRB140/urdf/meshes/link3.stl" />
       </geometry>
       <material
         name="">
@@ -157,7 +157,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link3.stl" />
+          filename="package://IRB140/urdf/meshes/link3.stl" />
       </geometry>
     </collision>
   </link>
@@ -183,7 +183,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link4.stl" />
+          filename="package://IRB140/urdf/meshes/link4.stl" />
       </geometry>
       <material
         name="">
@@ -197,7 +197,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link4.stl" />
+          filename="package://IRB140/urdf/meshes/link4.stl" />
       </geometry>
     </collision>
   </link>
@@ -223,7 +223,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link5.stl" />
+          filename="package://IRB140/urdf/meshes/link5.stl" />
       </geometry>
       <material
         name="">
@@ -237,7 +237,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link5.stl" />
+          filename="package://IRB140/urdf/meshes/link5.stl" />
       </geometry>
     </collision>
   </link>
@@ -263,7 +263,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link6.stl" />
+          filename="package://IRB140/urdf/meshes/link6.stl" />
       </geometry>
       <material
         name="">
@@ -277,7 +277,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link6.stl" />
+          filename="package://IRB140/urdf/meshes/link6.stl" />
       </geometry>
     </collision>
   </link>
diff --git a/examples/IRB140/urdf/irb_140_convhull.urdf b/examples/IRB140/urdf/irb_140_convhull.urdf
index df4514dec4db..411054540c73 100644
--- a/examples/IRB140/urdf/irb_140_convhull.urdf
+++ b/examples/IRB140/urdf/irb_140_convhull.urdf
@@ -23,7 +23,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/base_link_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/base_link_chull.wrl" />
       </geometry>
       <material
         name="">
@@ -37,7 +37,7 @@
         rpy="0 0 0" />
 <!--<geometry>
         <mesh
-          filename="meshes/base_link_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/base_link_chull.wrl" />
       </geometry>-->
     </collision>
   </link>
@@ -63,7 +63,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link1_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/link1_chull.wrl" />
       </geometry>
       <material
         name="">
@@ -77,7 +77,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link1_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/link1_chull.wrl" />
       </geometry>
     </collision>
   </link>
@@ -103,7 +103,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link2_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/link2_chull.wrl" />
       </geometry>
       <material
         name="">
@@ -117,7 +117,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link2_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/link2_chull.wrl" />
       </geometry>
     </collision>
   </link>
@@ -143,7 +143,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link3_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/link3_chull.wrl" />
       </geometry>
       <material
         name="">
@@ -157,7 +157,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link3_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/link3_chull.wrl" />
       </geometry>
     </collision>
   </link>
@@ -183,7 +183,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link4_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/link4_chull.wrl" />
       </geometry>
       <material
         name="">
@@ -197,7 +197,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link4_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/link4_chull.wrl" />
       </geometry>
     </collision>
   </link>
@@ -223,7 +223,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link5_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/link5_chull.wrl" />
       </geometry>
       <material
         name="">
@@ -237,7 +237,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link5_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/link5_chull.wrl" />
       </geometry>
     </collision>
   </link>
@@ -263,7 +263,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link6_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/link6_chull.wrl" />
       </geometry>
       <material
         name="">
@@ -277,7 +277,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="meshes/link6_chull.wrl" />
+          filename="package://IRB140/urdf/meshes/link6_chull.wrl" />
       </geometry>
     </collision>
   </link>
diff --git a/examples/IRB140/urdf/irb_140_robotiq.urdf b/examples/IRB140/urdf/irb_140_robotiq.urdf
index 9d507d8f408d..a2415f4bf268 100644
--- a/examples/IRB140/urdf/irb_140_robotiq.urdf
+++ b/examples/IRB140/urdf/irb_140_robotiq.urdf
@@ -1,983 +1,1284 @@
-<?xml version="1.0" ?>
-<robot
-  name="IRB140_-_M2004C_Complete_base.SLDASM">
-  <link
-    name="base_link">
-    <inertial>
-      <origin
-        xyz="-0.0820393251688425 -0.0599247343482246 0.000402028640224033"
-        rpy="0 0 0" />
-      <mass
-        value="12.409016159487" />
-      <inertia
-        ixx="0.104502176006253"
-        ixy="0.00734284655670482"
-        ixz="1.00520827530156E-05"
-        iyy="0.246486764065977"
-        iyz="0.000843006810157312"
-        izz="0.16727784258504" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/base_link.stl" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.890196078431373 0.411764705882353 0.125490196078431 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-<!--  <geometry>
-        <mesh
-          filename="meshes/base_link.stl" />
-      </geometry>-->
-    </collision>
-  </link>
-  <link
-    name="link_1">
-    <inertial>
-      <origin
-        xyz="0.0281052735195531 -0.263404021764231 0.0438145912936454"
-        rpy="0 0 0" />
-      <mass
-        value="16.287755696602" />
-      <inertia
-        ixx="0.239763088891016"
-        ixy="-0.0247090810274521"
-        ixz="0.000698689923332581"
-        iyy="0.215279593840589"
-        iyz="-0.0324602456686847"
-        izz="0.21738294278782" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0.1095 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link1.stl" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.890196078431373 0.411764705882353 0.125490196078431 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0.1095 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link1.stl" />
-      </geometry>
-    </collision>
-  </link>
-  <link
-    name="link_2">
-    <inertial>
-      <origin
-        xyz="0.0602722328661623 -0.550288012476887 -0.0924337165442594"
-        rpy="0 0 0" />
-      <mass
-        value="7.58037424484438" />
-      <inertia
-        ixx="0.155736578403723"
-        ixy="-0.0018292494088038"
-        ixz="-0.000516100043145016"
-        iyy="0.0449374006069967"
-        iyz="0.0179772597993246"
-        izz="0.131499062854069" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="-0.07 0.352 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link2.stl" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.890196078431373 0.411764705882353 0.125490196078431 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="-0.07 0.352 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link2.stl" />
-      </geometry>
-    </collision>
-  </link>
-  <link
-    name="link_3">
-    <inertial>
-      <origin
-        xyz="0.0895840773879386 -0.706321963929838 0.00750781548417993"
-        rpy="0 0 0" />
-      <mass
-        value="8.03072730997701" />
-      <inertia
-        ixx="0.0322819185861096"
-        ixy="0.00189304098071818"
-        ixz="-0.00301812650515099"
-        iyy="0.0862439596837411"
-        iyz="0.000363121583074651"
-        izz="0.0941344479457789" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="-0.07 0.712 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link3.stl" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.890196078431373 0.411764705882353 0.125490196078431 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="-0.07  0.712 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link3.stl" />
-      </geometry>
-    </collision>
-  </link>
-  <link
-    name="link_4">
-    <inertial>
-      <origin
-        xyz="0.381567778909365 -0.710566670346653 -0.0012125731023236"
-        rpy="0 0 0" />
-      <mass
-        value="1.7702548022269" />
-      <inertia
-        ixx="0.00345886724545012"
-        ixy="-5.89845314013543E-05"
-        ixz="-0.000204665310780048"
-        iyy="0.00814796317500158"
-        iyz="3.05102860299544E-06"
-        izz="0.00727417938725734" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="-0.2885 0.712 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link4.stl" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.890196078431373 0.411764705882353 0.125490196078431 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="-0.2885 0.712 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link4.stl" />
-      </geometry>
-    </collision>
-  </link>
-  <link
-    name="link_5">
-    <inertial>
-      <origin
-        xyz="0.4486171436144 -0.711838316531129 0.000617034654940496"
-        rpy="0 0 0" />
-      <mass
-        value="0.145550520315438" />
-      <inertia
-        ixx="7.1482542749252E-05"
-        ixy="4.08865519403599E-07"
-        ixz="1.24002244883737E-07"
-        iyy="6.88353185385481E-05"
-        iyz="-1.00702333671187E-08"
-        izz="0.000112132445217078" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="-0.450 0.712 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link5.stl" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.890196078431373 0.411764705882353 0.125490196078431 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="-0.450 0.712 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link5.stl" />
-      </geometry>
-    </collision>
-  </link>
-    <link
-    name="link_6">
-    <inertial>
-      <origin
-        xyz="0.502059432845099 -0.711774967143511 2.38785136319277E-10"
-        rpy="0 0 0" />
-      <mass
-        value="0.0258654328719498" />
-      <inertia
-        ixx="5.9822143548355E-06"
-        ixy="4.58244484401765E-08"
-        ixz="-3.33091906738059E-13"
-        iyy="6.2921914507958E-06"
-        iyz="6.75626369217644E-13"
-        izz="6.17479327327778E-06" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="-0.515 0.712 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link6.stl" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.796078431372549 0.823529411764706 0.937254901960784 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="-0.515 0.712 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="meshes/link6.stl" />
-      </geometry>
-    </collision>
-  </link>
-  <joint name="joint1" type="revolute">
-    <parent link="base_link"/>
-    <child link="link_1"/>
-    <limit effort="300" lower="-3.14159265359" upper="3.14159265359" velocity="4.36"/>
-    <axis xyz="0 -1 0"/>
-    <origin rpy="0 0 0" xyz="0 -0.1095 0"/>
-  </joint>
-  <joint name="joint2" type="revolute">
-    <parent link="link_1"/>
-    <child link="link_2"/>
-    <limit effort="300" lower="-1.57079633" upper="1.91986218" velocity="4.36"/>
-    <axis xyz="0 0 1"/>
-    <origin rpy="0 0 0" xyz="0.07 -0.2425 0"/>
-  </joint>
-  <joint name="joint3" type="revolute">
-    <parent link="link_2"/>
-    <child link="link_3"/>
-    <limit effort="300" lower="-4.01425728" upper="0.87266463" velocity="4.36"/>
-    <axis xyz="0 0 1"/>
-    <origin rpy="0 0 0" xyz="0 -0.360 0"/>
-  </joint>
-  <joint name="joint4" type="revolute">
-    <parent link="link_3"/>
-    <child link="link_4"/>
-    <limit effort="300" lower="-3.49065850" upper="3.49065850" velocity="5.58"/>
-    <axis xyz="1 0 0"/>
-    <origin rpy="0 0 0" xyz="0.2185 0 0"/>
-  </joint>
-  <joint name="joint5" type="revolute">
-    <parent link="link_4"/>
-    <child link="link_5"/>
-    <limit effort="300" lower="-2.09439510" upper="2.09439510" velocity="5.58"/>
-    <axis xyz="0 0 -1"/>
-    <origin rpy="0 0 0" xyz="0.1615 0 0"/>
-  </joint>
-  <joint name="joint6" type="revolute">
-    <parent link="link_5"/>
-    <child link="link_6"/>
-    <limit effort="300" lower="-6.98131701" upper="-6.98131701" velocity="7.33"/>
-    <axis xyz="1 0 0"/>
-    <origin rpy="0 0 0" xyz="0.065 0 0"/>
-  </joint>
-
-  <transmission name="joint1_trans" type="SimpleTransmission">
-    <joint name="joint1"/>
-    <actuator name="joint1_motor"/>
-    <mechanicalReduction>1</mechanicalReduction>
-  </transmission>
-
-  <transmission name="joint2_trans" type="SimpleTransmission">
-    <joint name="joint2"/>
-    <actuator name="joint2_motor"/>
-    <mechanicalReduction>1</mechanicalReduction>
-  </transmission>
-
-  <transmission name="joint3_trans" type="SimpleTransmission">
-    <joint name="joint3"/>
-    <actuator name="joint3_motor"/>
-    <mechanicalReduction>1</mechanicalReduction>
-  </transmission>
-
-  <transmission name="joint4_trans" type="SimpleTransmission">
-    <joint name="joint4"/>
-    <actuator name="joint4_motor"/>
-    <mechanicalReduction>1</mechanicalReduction>
-  </transmission>
-
-  <transmission name="joint5_trans" type="SimpleTransmission">
-    <joint name="joint5"/>
-    <actuator name="joint5_motor"/>
-    <mechanicalReduction>1</mechanicalReduction>
-  </transmission>
-  
-  <transmission name="joint6_trans" type="SimpleTransmission">
-    <joint name="joint6"/>
-    <actuator name="joint6_motor"/>
-    <mechanicalReduction>1</mechanicalReduction>
-  </transmission>
-
-<joint name="robotiq_hand_joint" type="fixed">
-    <origin rpy="0.0 0.0 -1.57079" xyz="0.11904 0 0"/>
-    <parent link="link_6"/>
-    <child link="palm"/>
-  </joint>
-  <link name="finger_1_link_0">
-    <visual>
-      <origin rpy="0 0 0" xyz="0.020 0 0"/>
-      <geometry>
-        <mesh filename="../robotiq_hand_description/meshes/s-model_articulated/visual/link_0.STL"/>
-      </geometry>
-      <material name="black">
-        <color rgba=".2 .2 .2 1"/>
-      </material>
-    </visual>
-    <!--<collision>	
-				<origin xyz="-0.0455 0.0414 0.036" rpy="0 3.1416 -1.57"/>
-				<geometry>
-					<mesh filename="../robotiq_hand_description/meshes/s-model_articulated/collision/link_0.STL" />
-				</geometry>
-				<material name="yellow">
-					<color rgba="0 1 1 1"/>
-				</material>
-			</collision>-->
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.05"/>
-      <inertia ixx="0.01" ixy="-0.00002" ixz="0.00001" iyy="0.0008" iyz="0" izz="0.001"/>
-    </inertial>
-  </link>
-  <link name="finger_1_link_1">
-    <visual>
-      <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
-      <geometry>
-        <mesh filename="../robotiq_hand_description/meshes/s-model_articulated/visual/link_1.STL"/>
-      </geometry>
-      <material name="black"/>
-    </visual>
-    <!--<collision>
-				<origin xyz="0.050 -.028 0" rpy="0 0 -0.52"/>
-				<geometry>
-					<mesh filename="../robotiq_hand_description/meshes/s-model_articulated/collision/link_1.STL" />
-				</geometry>
-				<material name="yellow"/>
-			</collision>-->
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.15"/>
-      <inertia ixx="0.001859" ixy="-0.000376" ixz="0.000028" iyy="0.012756" iyz="0" izz="0.0024"/>
-    </inertial>
-  </link>
-  <link name="finger_1_link_2">
-    <!--
-			NOTE: This link has an offset in the z-direction based on the models that were
-			used to generate grap-it poses.  With the offset below the model appears visually
-			correct, but it is suspect.
-			-->
-    <visual>
-      <origin rpy="0 0 0" xyz="0.039 0 0.0075"/>
-      <geometry>
-        <mesh filename="../robotiq_hand_description/meshes/s-model_articulated/visual/link_2.STL"/>
-      </geometry>
-      <material name="black"/>
-    </visual>
-    <!--<collision>
-				<origin xyz="0.039 0 0.0075" rpy="0 0 0"/>
-				<geometry>
-					<mesh filename="../robotiq_hand_description/meshes/s-model_articulated/collision/link_2.STL" />
-				</geometry>
-				<material name="yellow"/>
-			</collision>-->
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.15"/>
-      <inertia ixx="0.001859" ixy="-0.000376" ixz="0.000028" iyy="0.012756" iyz="0" izz="0.0024"/>
-    </inertial>
-  </link>
-  <link name="finger_1_link_3">
-    <visual>
-      <origin rpy="0 0 0.52" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="../robotiq_hand_description/meshes/s-model_articulated/visual/link_3.STL"/>
-      </geometry>
-      <material name="black"/>
-    </visual>
-    <!--<collision>
-				<origin xyz="0 0 0" rpy="0 0 0.52"/>
-				<geometry>
-					<mesh filename="../robotiq_hand_description/meshes/s-model_articulated/collision/link_3.STL" />
-				</geometry>
-				<material name="yellow"/>
-			</collision>-->
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.05"/>
-      <inertia ixx="0.001239" ixy="-0.000251" ixz="0.000019" iyy="0.00085" iyz="0" izz="0.001632"/>
-    </inertial>
-  </link>
-  <link name="finger_1_fourbar_0_link_0">
-    <inertial>
-      <mass value="0.000001"/>
-      <inertia ixx="0.00000000001" ixy="0" ixz="0" iyy="0.00000000001" iyz="0" izz="0.00000000001"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 1.57 0" xyz="0.01429 0 0"/>
-			<!--<geometry>
-        <cylinder length="0.02858" radius="0.01"/>
-			</geometry>-->
-    </visual>
-  </link>
-  <link name="finger_1_fourbar_0_link_1">
-    <inertial>
-      <mass value="0.000001"/>
-      <inertia ixx="0.00000000001" ixy="0" ixz="0" iyy="0.00000000001" iyz="0" izz="0.00000000001"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 1.57 0" xyz="0.02699 0 0"/>
-			<!--<geometry>
-        <cylinder length="0.05398" radius="0.01"/>
-			</geometry>-->
-    </visual>
-  </link>
-  <link name="finger_1_fourbar_0_link_2">
-    <inertial>
-      <mass value="0.000001"/>
-      <inertia ixx="0.00000000001" ixy="0" ixz="0" iyy="0.00000000001" iyz="0" izz="0.00000000001"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 1.57 0" xyz="0.007365 0 0"/>
-			<!--<geometry>
-        <cylinder length="0.01473" radius="0.01"/>
-			</geometry>-->
-    </visual>
-  </link>
-  <joint name="finger_1_fourbar_0_joint_1" type="continuous">
-    <parent link="finger_1_link_1"/>
-    <child link="finger_1_fourbar_0_link_0"/>
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <joint name="finger_1_fourbar_0_joint_2" type="continuous">
-    <parent link="finger_1_fourbar_0_link_0"/>
-    <child link="finger_1_fourbar_0_link_1"/>
-    <origin rpy="0 0 0" xyz="0.02858 0 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <joint name="finger_1_fourbar_0_joint_3" type="continuous">
-    <parent link="finger_1_fourbar_0_link_1"/>
-    <child link="finger_1_fourbar_0_link_2"/>
-    <origin rpy="0 0 0" xyz="0.05398 0 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <loop_joint name="finger_1_fourbar_0_joint_4" type="continuous">
-    <link1 link="finger_1_fourbar_0_link_1" rpy="0 0 0" xyz="0 0 0"/>
-    <link2 link="finger_1_link_2" rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="0 0 1"/>
-  </loop_joint>
-  <joint name="finger_1_joint_1" type="revolute">
-    <parent link="finger_1_link_0"/>
-    <child link="finger_1_link_1"/>
-    <axis xyz="0 0 1"/>
-    <origin rpy="0 0 0" xyz="0.020 0 0"/>
-    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
-  </joint>
-  <joint name="finger_1_joint_2" type="revolute">
-    <parent link="finger_1_link_1"/>
-    <child link="finger_1_link_2"/>
-    <axis xyz="0 0 1"/>
-    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
-    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
-  </joint>
-  <joint name="finger_1_joint_3" type="revolute">
-    <parent link="finger_1_link_2"/>
-    <child link="finger_1_link_3"/>
-    <axis xyz="0 0 1"/>
-    <origin rpy="0 0 0" xyz="0.039 0 0"/>
-    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
-  </joint>
-  <gazebo reference="finger_1_link_0">
-    <material>Gazebo/FlatBlack</material>
-    <turnGravityOff>true</turnGravityOff>
-  </gazebo>
-  <gazebo reference="finger_1_link_1">
-    <material>Gazebo/FlatBlack</material>
-    <turnGravityOff>true</turnGravityOff>
-  </gazebo>
-  <gazebo reference="finger_1_link_2">
-    <material>Gazebo/FlatBlack</material>
-    <turnGravityOff>true</turnGravityOff>
-  </gazebo>
-  <gazebo reference="finger_1_link_3">
-    <material>Gazebo/FlatBlack</material>
-    <turnGravityOff>true</turnGravityOff>
-  </gazebo>
-  <link name="finger_2_link_0">
-    <visual>
-      <origin rpy="0 0 0" xyz="0.020 0 0"/>
-      <geometry>
-        <mesh filename="../robotiq_hand_description/meshes/s-model_articulated/visual/link_0.STL"/>
-      </geometry>
-      <material name="black">
-        <color rgba=".2 .2 .2 1"/>
-      </material>
-    </visual>
-    <!--<collision>	
-				<origin xyz="-0.0455 0.0414 0.036" rpy="0 3.1416 -1.57"/>
-				<geometry>
-					<mesh filename="../robotiq_hand_description/meshes/s-model_articulated/collision/link_0.STL" />
-				</geometry>
-				<material name="yellow">
-					<color rgba="0 1 1 1"/>
-				</material>
-			</collision>-->
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.05"/>
-      <inertia ixx="0.01" ixy="-0.00002" ixz="0.00001" iyy="0.0008" iyz="0" izz="0.001"/>
-    </inertial>
-  </link>
-  <link name="finger_2_link_1">
-    <visual>
-      <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
-      <geometry>
-        <mesh filename="../robotiq_hand_description/meshes/s-model_articulated/visual/link_1.STL"/>
-      </geometry>
-      <material name="black"/>
-    </visual>
-    <!--<collision>
-				<origin xyz="0.050 -.028 0" rpy="0 0 -0.52"/>
-				<geometry>
-					<mesh filename="../robotiq_hand_description/meshes/s-model_articulated/collision/link_1.STL" />
-				</geometry>
-				<material name="yellow"/>
-			</collision>-->
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.15"/>
-      <inertia ixx="0.001859" ixy="-0.000376" ixz="0.000028" iyy="0.012756" iyz="0" izz="0.0024"/>
-    </inertial>
-  </link>
-  <link name="finger_2_link_2">
-    <!--
-			NOTE: This link has an offset in the z-direction based on the models that were
-			used to generate grap-it poses.  With the offset below the model appears visually
-			correct, but it is suspect.
-			-->
-    <visual>
-      <origin rpy="0 0 0" xyz="0.039 0 0.0075"/>
-      <geometry>
-        <mesh filename="../robotiq_hand_description/meshes/s-model_articulated/visual/link_2.STL"/>
-      </geometry>
-      <material name="black"/>
-    </visual>
-    <!--<collision>
-				<origin xyz="0.039 0 0.0075" rpy="0 0 0"/>
-				<geometry>
-					<mesh filename="../robotiq_hand_description/meshes/s-model_articulated/collision/link_2.STL" />
-				</geometry>
-				<material name="yellow"/>
-			</collision>-->
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.15"/>
-      <inertia ixx="0.001859" ixy="-0.000376" ixz="0.000028" iyy="0.012756" iyz="0" izz="0.0024"/>
-    </inertial>
-  </link>
-  <link name="finger_2_link_3">
-    <visual>
-      <origin rpy="0 0 0.52" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="../robotiq_hand_description/meshes/s-model_articulated/visual/link_3.STL"/>
-      </geometry>
-      <material name="black"/>
-    </visual>
-    <!--<collision>
-				<origin xyz="0 0 0" rpy="0 0 0.52"/>
-				<geometry>
-					<mesh filename="../robotiq_hand_description/meshes/s-model_articulated/collision/link_3.STL" />
-				</geometry>
-				<material name="yellow"/>
-			</collision>-->
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.05"/>
-      <inertia ixx="0.001239" ixy="-0.000251" ixz="0.000019" iyy="0.00085" iyz="0" izz="0.001632"/>
-    </inertial>
-  </link>
-  <link name="finger_2_fourbar_0_link_0">
-    <inertial>
-      <mass value="0.000001"/>
-      <inertia ixx="0.00000000001" ixy="0" ixz="0" iyy="0.00000000001" iyz="0" izz="0.00000000001"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 1.57 0" xyz="0.01429 0 0"/>
-			<!--<geometry>
-        <cylinder length="0.02858" radius="0.01"/>
-			</geometry>-->
-    </visual>
-  </link>
-  <link name="finger_2_fourbar_0_link_1">
-    <inertial>
-      <mass value="0.000001"/>
-      <inertia ixx="0.00000000001" ixy="0" ixz="0" iyy="0.00000000001" iyz="0" izz="0.00000000001"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 1.57 0" xyz="0.02699 0 0"/>
-			<!--<geometry>
-        <cylinder length="0.05398" radius="0.01"/>
-			</geometry>-->
-    </visual>
-  </link>
-  <link name="finger_2_fourbar_0_link_2">
-    <inertial>
-      <mass value="0.000001"/>
-      <inertia ixx="0.00000000001" ixy="0" ixz="0" iyy="0.00000000001" iyz="0" izz="0.00000000001"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 1.57 0" xyz="0.007365 0 0"/>
-			<!--<geometry>
-        <cylinder length="0.01473" radius="0.01"/>
-			</geometry>-->
-    </visual>
-  </link>
-  <joint name="finger_2_fourbar_0_joint_1" type="continuous">
-    <parent link="finger_2_link_1"/>
-    <child link="finger_2_fourbar_0_link_0"/>
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <joint name="finger_2_fourbar_0_joint_2" type="continuous">
-    <parent link="finger_2_fourbar_0_link_0"/>
-    <child link="finger_2_fourbar_0_link_1"/>
-    <origin rpy="0 0 0" xyz="0.02858 0 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <joint name="finger_2_fourbar_0_joint_3" type="continuous">
-    <parent link="finger_2_fourbar_0_link_1"/>
-    <child link="finger_2_fourbar_0_link_2"/>
-    <origin rpy="0 0 0" xyz="0.05398 0 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <loop_joint name="finger_2_fourbar_0_joint_4" type="continuous">
-    <link1 link="finger_2_fourbar_0_link_1" rpy="0 0 0" xyz="0 0 0"/>
-    <link2 link="finger_2_link_2" rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="0 0 1"/>
-  </loop_joint>
-  <joint name="finger_2_joint_1" type="revolute">
-    <parent link="finger_2_link_0"/>
-    <child link="finger_2_link_1"/>
-    <axis xyz="0 0 1"/>
-    <origin rpy="0 0 0" xyz="0.020 0 0"/>
-    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
-  </joint>
-  <joint name="finger_2_joint_2" type="revolute">
-    <parent link="finger_2_link_1"/>
-    <child link="finger_2_link_2"/>
-    <axis xyz="0 0 1"/>
-    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
-    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
-  </joint>
-  <joint name="finger_2_joint_3" type="revolute">
-    <parent link="finger_2_link_2"/>
-    <child link="finger_2_link_3"/>
-    <axis xyz="0 0 1"/>
-    <origin rpy="0 0 0" xyz="0.039 0 0"/>
-    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
-  </joint>
-  <gazebo reference="finger_2_link_0">
-    <material>Gazebo/FlatBlack</material>
-    <turnGravityOff>true</turnGravityOff>
-  </gazebo>
-  <gazebo reference="finger_2_link_1">
-    <material>Gazebo/FlatBlack</material>
-    <turnGravityOff>true</turnGravityOff>
-  </gazebo>
-  <gazebo reference="finger_2_link_2">
-    <material>Gazebo/FlatBlack</material>
-    <turnGravityOff>true</turnGravityOff>
-  </gazebo>
-  <gazebo reference="finger_2_link_3">
-    <material>Gazebo/FlatBlack</material>
-    <turnGravityOff>true</turnGravityOff>
-  </gazebo>
-  <link name="finger_middle_link_0">
-    <visual>
-      <origin rpy="0 0 0" xyz="0.020 0 0"/>
-      <geometry>
-        <mesh filename="../robotiq_hand_description/meshes/s-model_articulated/visual/link_0.STL"/>
-      </geometry>
-      <material name="black">
-        <color rgba=".2 .2 .2 1"/>
-      </material>
-    </visual>
-    <!--<collision>	
-				<origin xyz="-0.0455 0.0414 0.036" rpy="0 3.1416 -1.57"/>
-				<geometry>
-					<mesh filename="../robotiq_hand_description/meshes/s-model_articulated/collision/link_0.STL" />
-				</geometry>
-				<material name="yellow">
-					<color rgba="0 1 1 1"/>
-				</material>
-			</collision>-->
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.05"/>
-      <inertia ixx="0.01" ixy="-0.00002" ixz="0.00001" iyy="0.0008" iyz="0" izz="0.001"/>
-    </inertial>
-  </link>
-  <link name="finger_middle_link_1">
-    <visual>
-      <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
-      <geometry>
-        <mesh filename="../robotiq_hand_description/meshes/s-model_articulated/visual/link_1.STL"/>
-      </geometry>
-      <material name="black"/>
-    </visual>
-    <!--<collision>
-				<origin xyz="0.050 -.028 0" rpy="0 0 -0.52"/>
-				<geometry>
-					<mesh filename="../robotiq_hand_description/meshes/s-model_articulated/collision/link_1.STL" />
-				</geometry>
-				<material name="yellow"/>
-			</collision>-->
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.15"/>
-      <inertia ixx="0.001859" ixy="-0.000376" ixz="0.000028" iyy="0.012756" iyz="0" izz="0.0024"/>
-    </inertial>
-  </link>
-  <link name="finger_middle_link_2">
-    <!--
-			NOTE: This link has an offset in the z-direction based on the models that were
-			used to generate grap-it poses.  With the offset below the model appears visually
-			correct, but it is suspect.
-			-->
-    <visual>
-      <origin rpy="0 0 0" xyz="0.039 0 0.0075"/>
-      <geometry>
-        <mesh filename="../robotiq_hand_description/meshes/s-model_articulated/visual/link_2.STL"/>
-      </geometry>
-      <material name="black"/>
-    </visual>
-    <!--<collision>
-				<origin xyz="0.039 0 0.0075" rpy="0 0 0"/>
-				<geometry>
-					<mesh filename="../robotiq_hand_description/meshes/s-model_articulated/collision/link_2.STL" />
-				</geometry>
-				<material name="yellow"/>
-			</collision>-->
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.15"/>
-      <inertia ixx="0.001859" ixy="-0.000376" ixz="0.000028" iyy="0.012756" iyz="0" izz="0.0024"/>
-    </inertial>
-  </link>
-  <link name="finger_middle_link_3">
-    <visual>
-      <origin rpy="0 0 0.52" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="../robotiq_hand_description/meshes/s-model_articulated/visual/link_3.STL"/>
-      </geometry>
-      <material name="black"/>
-    </visual>
-    <!--<collision>
-				<origin xyz="0 0 0" rpy="0 0 0.52"/>
-				<geometry>
-					<mesh filename="../robotiq_hand_description/meshes/s-model_articulated/collision/link_3.STL" />
-				</geometry>
-				<material name="yellow"/>
-			</collision>-->
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.05"/>
-      <inertia ixx="0.001239" ixy="-0.000251" ixz="0.000019" iyy="0.00085" iyz="0" izz="0.001632"/>
-    </inertial>
-  </link>
-  <link name="finger_middle_fourbar_0_link_0">
-    <inertial>
-      <mass value="0.000001"/>
-      <inertia ixx="0.00000000001" ixy="0" ixz="0" iyy="0.00000000001" iyz="0" izz="0.00000000001"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 1.57 0" xyz="0.01429 0 0"/>
-			<!--<geometry>
-        <cylinder length="0.02858" radius="0.01"/>
-			</geometry>-->
-    </visual>
-  </link>
-  <link name="finger_middle_fourbar_0_link_1">
-    <inertial>
-      <mass value="0.000001"/>
-      <inertia ixx="0.00000000001" ixy="0" ixz="0" iyy="0.00000000001" iyz="0" izz="0.00000000001"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 1.57 0" xyz="0.02699 0 0"/>
-			<!--<geometry>
-        <cylinder length="0.05398" radius="0.01"/>
-			</geometry>-->
-    </visual>
-  </link>
-  <link name="finger_middle_fourbar_0_link_2">
-    <inertial>
-      <mass value="0.000001"/>
-      <inertia ixx="0.00000000001" ixy="0" ixz="0" iyy="0.00000000001" iyz="0" izz="0.00000000001"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 1.57 0" xyz="0.007365 0 0"/>
-			<!--<geometry>
-        <cylinder length="0.01473" radius="0.01"/>
-			</geometry>-->
-    </visual>
-  </link>
-  <joint name="finger_middle_fourbar_0_joint_1" type="continuous">
-    <parent link="finger_middle_link_1"/>
-    <child link="finger_middle_fourbar_0_link_0"/>
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <joint name="finger_middle_fourbar_0_joint_2" type="continuous">
-    <parent link="finger_middle_fourbar_0_link_0"/>
-    <child link="finger_middle_fourbar_0_link_1"/>
-    <origin rpy="0 0 0" xyz="0.02858 0 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <joint name="finger_middle_fourbar_0_joint_3" type="continuous">
-    <parent link="finger_middle_fourbar_0_link_1"/>
-    <child link="finger_middle_fourbar_0_link_2"/>
-    <origin rpy="0 0 0" xyz="0.05398 0 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <loop_joint name="finger_middle_fourbar_0_joint_4" type="continuous">
-    <link1 link="finger_middle_fourbar_0_link_1" rpy="0 0 0" xyz="0 0 0"/>
-    <link2 link="finger_middle_link_2" rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="0 0 1"/>
-  </loop_joint>
-  <joint name="finger_middle_joint_1" type="revolute">
-    <parent link="finger_middle_link_0"/>
-    <child link="finger_middle_link_1"/>
-    <axis xyz="0 0 1"/>
-    <origin rpy="0 0 0" xyz="0.020 0 0"/>
-    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
-  </joint>
-  <joint name="finger_middle_joint_2" type="revolute">
-    <parent link="finger_middle_link_1"/>
-    <child link="finger_middle_link_2"/>
-    <axis xyz="0 0 1"/>
-    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
-    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
-  </joint>
-  <joint name="finger_middle_joint_3" type="revolute">
-    <parent link="finger_middle_link_2"/>
-    <child link="finger_middle_link_3"/>
-    <axis xyz="0 0 1"/>
-    <origin rpy="0 0 0" xyz="0.039 0 0"/>
-    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
-  </joint>
-  <gazebo reference="finger_middle_link_0">
-    <material>Gazebo/FlatBlack</material>
-    <turnGravityOff>true</turnGravityOff>
-  </gazebo>
-  <gazebo reference="finger_middle_link_1">
-    <material>Gazebo/FlatBlack</material>
-    <turnGravityOff>true</turnGravityOff>
-  </gazebo>
-  <gazebo reference="finger_middle_link_2">
-    <material>Gazebo/FlatBlack</material>
-    <turnGravityOff>true</turnGravityOff>
-  </gazebo>
-  <gazebo reference="finger_middle_link_3">
-    <material>Gazebo/FlatBlack</material>
-    <turnGravityOff>true</turnGravityOff>
-  </gazebo>
-  <link name="palm">
-    <visual>
-      <geometry>
-        <mesh filename="../robotiq_hand_description/meshes/s-model_articulated/visual/palm.STL"/>
-      </geometry>
-      <material name="black">
-        <color rgba=".2 .2 .2 1"/>
-      </material>
-    </visual>
-    <collision>
-		<geometry>
-			<mesh filename="../robotiq_hand_description/meshes/s-model_articulated/collision/palm.STL" />
-		</geometry>
-		<material name="yellow">
-			<color rgba="0 1 1 1"/>
-		</material>
-	</collision>
-  </link>
-  <inertial>
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <mass value="1.3"/>
-    <inertia ixx="0.006012" ixy="0.000079" ixz="-0.00024" iyy="0.012892" iyz="0" izz="0.002435"/>
-  </inertial>
-  <joint name="palm_finger_1_joint" type="revolute">
-    <parent link="palm"/>
-    <child link="finger_1_link_0"/>
-    <axis xyz="0 1 0"/>
-    <origin rpy="0 3.1416 -1.57" xyz="-0.0455 0.0214 0.036"/>
-    <limit effort="100" lower="-0.16" upper="0.25" velocity="100"/>
-  </joint>
-  <joint name="palm_finger_2_joint" type="revolute">
-    <parent link="palm"/>
-    <child link="finger_2_link_0"/>
-    <axis xyz="0 1 0"/>
-    <origin rpy="0 3.1416 -1.57" xyz="-0.0455 0.0214 -0.036"/>
-    <limit effort="100" lower="-0.25" upper="0.16" velocity="100"/>
-  </joint>
-  <joint name="palm_finger_middle_joint" type="fixed">
-    <parent link="palm"/>
-    <child link="finger_middle_link_0"/>
-    <axis xyz="0 0 1"/>
-    <origin rpy="0 0 1.57" xyz="0.0455 0.0214 0"/>
-  </joint>
-  <gazebo reference="palm">
-    <material>Gazebo/FlatBlack</material>
-    <turnGravityOff>true</turnGravityOff>
-  </gazebo>
-  
-</robot>
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from irb_140_robotiq.xacro          | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="IRB140" xmlns:xacro="http://ros.org/wiki/xacro">
+  <link name="base_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0820393251688425 -0.0599247343482246 0.000402028640224033"/>
+      <mass value="12.409016159487"/>
+      <inertia ixx="0.104502176006253" ixy="0.00734284655670482" ixz="1.00520827530156E-05" iyy="0.246486764065977" iyz="0.000843006810157312" izz="0.16727784258504"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!--<geometry>
+        <mesh
+          filename="package://IRB140/urdf/meshes/base_link.stl" />
+      </geometry>-->
+      <material name="">
+        <color rgba="0.890196078431373 0.411764705882353 0.125490196078431 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://IRB140/urdf/meshes/base_link.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_1">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0281052735195531 -0.263404021764231 0.0438145912936454"/>
+      <mass value="16.287755696602"/>
+      <inertia ixx="0.239763088891016" ixy="-0.0247090810274521" ixz="0.000698689923332581" iyy="0.215279593840589" iyz="-0.0324602456686847" izz="0.21738294278782"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0.1095 0"/>
+      <geometry>
+        <mesh filename="package://IRB140/urdf/meshes/link1.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431373 0.411764705882353 0.125490196078431 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0.1095 0"/>
+      <geometry>
+        <mesh filename="package://IRB140/urdf/meshes/link1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_2">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0602722328661623 -0.550288012476887 -0.0924337165442594"/>
+      <mass value="7.58037424484438"/>
+      <inertia ixx="0.155736578403723" ixy="-0.0018292494088038" ixz="-0.000516100043145016" iyy="0.0449374006069967" iyz="0.0179772597993246" izz="0.131499062854069"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.07 0.352 0"/>
+      <geometry>
+        <mesh filename="package://IRB140/urdf/meshes/link2.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431373 0.411764705882353 0.125490196078431 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.07 0.352 0"/>
+      <geometry>
+        <mesh filename="package://IRB140/urdf/meshes/link2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_3">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0895840773879386 -0.706321963929838 0.00750781548417993"/>
+      <mass value="8.03072730997701"/>
+      <inertia ixx="0.0322819185861096" ixy="0.00189304098071818" ixz="-0.00301812650515099" iyy="0.0862439596837411" iyz="0.000363121583074651" izz="0.0941344479457789"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.07 0.712 0"/>
+      <geometry>
+        <mesh filename="package://IRB140/urdf/meshes/link3.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431373 0.411764705882353 0.125490196078431 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.07  0.712 0"/>
+      <geometry>
+        <mesh filename="package://IRB140/urdf/meshes/link3.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_4">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.381567778909365 -0.710566670346653 -0.0012125731023236"/>
+      <mass value="1.7702548022269"/>
+      <inertia ixx="0.00345886724545012" ixy="-5.89845314013543E-05" ixz="-0.000204665310780048" iyy="0.00814796317500158" iyz="3.05102860299544E-06" izz="0.00727417938725734"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.2885 0.712 0"/>
+      <geometry>
+        <mesh filename="package://IRB140/urdf/meshes/link4.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431373 0.411764705882353 0.125490196078431 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.2885 0.712 0"/>
+      <geometry>
+        <mesh filename="package://IRB140/urdf/meshes/link4.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_5">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.4486171436144 -0.711838316531129 0.000617034654940496"/>
+      <mass value="0.145550520315438"/>
+      <inertia ixx="7.1482542749252E-05" ixy="4.08865519403599E-07" ixz="1.24002244883737E-07" iyy="6.88353185385481E-05" iyz="-1.00702333671187E-08" izz="0.000112132445217078"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.450 0.712 0"/>
+      <geometry>
+        <mesh filename="package://IRB140/urdf/meshes/link5.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431373 0.411764705882353 0.125490196078431 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.450 0.712 0"/>
+      <geometry>
+        <mesh filename="package://IRB140/urdf/meshes/link5.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_6">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.502059432845099 -0.711774967143511 2.38785136319277E-10"/>
+      <mass value="0.0258654328719498"/>
+      <inertia ixx="5.9822143548355E-06" ixy="4.58244484401765E-08" ixz="-3.33091906738059E-13" iyy="6.2921914507958E-06" iyz="6.75626369217644E-13" izz="6.17479327327778E-06"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.515 0.712 0"/>
+      <geometry>
+        <mesh filename="package://IRB140/urdf/meshes/link6.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.796078431372549 0.823529411764706 0.937254901960784 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.515 0.712 0"/>
+      <geometry>
+        <mesh filename="package://IRB140/urdf/meshes/link6.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint1" type="revolute">
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <limit effort="300" lower="-3.14159265359" upper="3.14159265359" velocity="4.36"/>
+    <axis xyz="0 -1 0"/>
+    <origin rpy="0 0 0" xyz="0 -0.1095 0"/>
+  </joint>
+  <joint name="joint2" type="revolute">
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <limit effort="300" lower="-1.57079633" upper="1.91986218" velocity="4.36"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.07 -0.2425 0"/>
+  </joint>
+  <joint name="joint3" type="revolute">
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <limit effort="300" lower="-4.01425728" upper="0.87266463" velocity="4.36"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0 -0.360 0"/>
+  </joint>
+  <joint name="joint4" type="revolute">
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <limit effort="300" lower="-3.49065850" upper="3.49065850" velocity="5.58"/>
+    <axis xyz="1 0 0"/>
+    <origin rpy="0 0 0" xyz="0.2185 0 0"/>
+  </joint>
+  <joint name="joint5" type="revolute">
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <limit effort="300" lower="-2.09439510" upper="2.09439510" velocity="5.58"/>
+    <axis xyz="0 0 -1"/>
+    <origin rpy="0 0 0" xyz="0.1615 0 0"/>
+  </joint>
+  <joint name="joint6" type="revolute">
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <limit effort="300" lower="-6.98131701" upper="-6.98131701" velocity="7.33"/>
+    <axis xyz="1 0 0"/>
+    <origin rpy="0 0 0" xyz="0.065 0 0"/>
+  </joint>
+  <transmission name="joint1_trans" type="SimpleTransmission">
+    <joint name="joint1"/>
+    <actuator name="joint1_motor"/>
+    <mechanicalReduction>1</mechanicalReduction>
+  </transmission>
+  <transmission name="joint2_trans" type="SimpleTransmission">
+    <joint name="joint2"/>
+    <actuator name="joint2_motor"/>
+    <mechanicalReduction>1</mechanicalReduction>
+  </transmission>
+  <transmission name="joint3_trans" type="SimpleTransmission">
+    <joint name="joint3"/>
+    <actuator name="joint3_motor"/>
+    <mechanicalReduction>1</mechanicalReduction>
+  </transmission>
+  <transmission name="joint4_trans" type="SimpleTransmission">
+    <joint name="joint4"/>
+    <actuator name="joint4_motor"/>
+    <mechanicalReduction>1</mechanicalReduction>
+  </transmission>
+  <transmission name="joint5_trans" type="SimpleTransmission">
+    <joint name="joint5"/>
+    <actuator name="joint5_motor"/>
+    <mechanicalReduction>1</mechanicalReduction>
+  </transmission>
+  <transmission name="joint6_trans" type="SimpleTransmission">
+    <joint name="joint6"/>
+    <actuator name="joint6_motor"/>
+    <mechanicalReduction>1</mechanicalReduction>
+  </transmission>
+  <link name="finger_1_link_0">
+    <visual>
+      <origin rpy="0 0 0" xyz="0.020 0 0"/>
+      <geometry>
+        <mesh filename="package://Atlas/urdf/meshes/s-model_articulated/visual/link_0.STL"/>
+      </geometry>
+      <material name="black">
+        <color rgba=".2 .2 .2 1"/>
+      </material>
+    </visual>
+    <!--<collision>	
+				<origin xyz="-0.0455 0.0414 0.036" rpy="0 3.1416 -1.57"/>
+				<geometry>
+					<mesh filename="package://Atlas/urdf/meshes/s-model_articulated/collision/link_0.STL" />
+				</geometry>
+				<material name="yellow">
+					<color rgba="0 1 1 1"/>
+				</material>
+			</collision>-->
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.01" ixy="-0.00002" ixz="0.00001" iyy="0.0008" iyz="0" izz="0.001"/>
+    </inertial>
+  </link>
+  <link name="finger_1_link_1">
+    <visual>
+      <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+      <geometry>
+        <mesh filename="package://Atlas/urdf/meshes/s-model_articulated/visual/link_1.STL"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.52" xyz="0.03 -0.0145 0"/>
+      <geometry>
+        <box size=".028 .01 .03"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.15"/>
+      <inertia ixx="0.001859" ixy="-0.000376" ixz="0.000028" iyy="0.012756" iyz="0" izz="0.0024"/>
+    </inertial>
+  </link>
+  <link name="finger_1_link_2">
+    <!--
+			NOTE: This link has an offset in the z-direction based on the models that were
+			used to generate grap-it poses.  With the offset below the model appears visually
+			correct, but it is suspect.
+			-->
+    <visual>
+      <origin rpy="0 0 0" xyz="0.039 0 0.0075"/>
+      <geometry>
+        <mesh filename="package://Atlas/urdf/meshes/s-model_articulated/visual/link_2.STL"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.02 0.0025 -0.0005"/>
+      <geometry>
+        <box size=".018 .01 .03"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.15"/>
+      <inertia ixx="0.001859" ixy="-0.000376" ixz="0.000028" iyy="0.012756" iyz="0" izz="0.0024"/>
+    </inertial>
+  </link>
+  <link name="finger_1_link_3">
+    <visual>
+      <origin rpy="0 0 0.52" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://Atlas/urdf/meshes/s-model_articulated/visual/link_3.STL"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 .52" xyz="0.02 0.015 0"/>
+      <geometry>
+        <box size=".025 .01 .03"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.001239" ixy="-0.000251" ixz="0.000019" iyy="0.00085" iyz="0" izz="0.001632"/>
+    </inertial>
+  </link>
+  <link name="finger_1_fourbar_0_link_1">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".01429 0 0"/>
+      <geometry>
+        <cylinder length=".02858" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_1_fourbar_0_link_2">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".02699 0 0"/>
+      <geometry>
+        <cylinder length=".05398" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_1_fourbar_0_link_3">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".007365 0 0"/>
+      <geometry>
+        <cylinder length=".01473" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_1_fourbar_3_link_1">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".01111 0 0"/>
+      <geometry>
+        <cylinder length=".02222" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_1_fourbar_3_link_2">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".01905 0 0"/>
+      <geometry>
+        <cylinder length=".03810" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_1_fourbar_3_link_3">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".00635 0 0"/>
+      <geometry>
+        <cylinder length=".01270" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_1_fourbar_linkage">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".007618 0 0"/>
+      <geometry>
+        <cylinder length=".015236" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="finger_1_joint_0" type="revolute">
+    <parent link="finger_1_link_0"/>
+    <child link="finger_1_link_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.020 0 0"/>
+    <limit effort="100" lower="0" upper="1.055" velocity="100"/>
+  </joint>
+  <joint name="finger_1_joint_1" type="revolute">
+    <parent link="finger_1_link_1"/>
+    <child link="finger_1_link_2"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+    <limit effort="100" lower="0" upper="1.5708" velocity="100"/>
+  </joint>
+  <joint name="finger_1_joint_2" type="revolute">
+    <parent link="finger_1_link_2"/>
+    <child link="finger_1_link_3"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.039 0 0"/>
+    <limit effort="100" lower="-0.7854" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="finger_1_fourbar_0_joint_0" type="revolute">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_1_link_0"/>
+    <child link="finger_1_fourbar_0_link_1"/>
+    <origin rpy="0 0 0" xyz="0.020 0 0"/>
+    <limit effort="100" lower="-2.10" upper="-0.255" velocity="100"/>
+  </joint>
+  <joint name="finger_1_fourbar_0_joint_1" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_1_fourbar_0_link_1"/>
+    <child link="finger_1_fourbar_0_link_2"/>
+    <origin rpy="0 0 0" xyz=".02858 0 0"/>
+    <limit effort="100" lower="1.092" upper="1.940" velocity="100"/>
+  </joint>
+  <joint name="finger_1_fourbar_0_joint_2" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_1_link_1"/>
+    <child link="finger_1_fourbar_0_link_3"/>
+    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+    <limit effort="100" lower="-2.111" upper="-0.199" velocity="100"/>
+  </joint>
+  <loop_joint name="finger_1_fourbar_0_joint_3" type="continuous">
+    <axis xyz="0 0 1"/>
+    <link1 link="finger_1_fourbar_0_link_2" rpy="0 0 0" xyz=".05398 0 0"/>
+    <link2 link="finger_1_fourbar_0_link_3" rpy="0 0 0" xyz=".01473 0 0"/>
+  </loop_joint>
+  <joint name="finger_1_fourbar_3_joint_0" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_1_link_2"/>
+    <child link="finger_1_fourbar_3_link_1"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <limit effort="100" lower="-1.367" upper="-0.945" velocity="100"/>
+  </joint>
+  <joint name="finger_1_fourbar_3_joint_1" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_1_fourbar_3_link_1"/>
+    <child link="finger_1_fourbar_3_link_2"/>
+    <origin rpy="0 0 0" xyz="0.02222 0 0"/>
+    <limit effort="100" lower="1.235" upper="1.613" velocity="100"/>
+  </joint>
+  <joint name="finger_1_fourbar_3_joint_2" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_1_link_3"/>
+    <child link="finger_1_fourbar_3_link_3"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <limit effort="100" lower="-1.367" upper="-0.995" velocity="100"/>
+  </joint>
+  <loop_joint name="finger_1_fourbar_3_joint_3" type="continuous">
+    <axis xyz="0 0 1"/>
+    <link1 link="finger_1_fourbar_3_link_3" rpy="0 0 0" xyz="0.01270 0 0"/>
+    <link2 link="finger_1_fourbar_3_link_2" rpy="0 0 0" xyz="0.03810 0 0"/>
+  </loop_joint>
+  <joint name="finger_1_fourbar_linkage_joint_0" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_1_fourbar_0_link_3"/>
+    <child link="finger_1_fourbar_linkage"/>
+    <origin rpy="0 0 0" xyz=".01473 0 0"/>
+    <limit effort="100" lower="1.4" upper="1.5" velocity="100"/>
+  </joint>
+  <loop_joint name="finger_1_fourbar_linkage_joint_1" type="continuous">
+    <axis xyz="0 0 1"/>
+    <link1 link="finger_1_fourbar_linkage" rpy="0 0 0" xyz=".015236 0 0"/>
+    <link2 link="finger_1_fourbar_3_link_1" rpy="0 0 0" xyz=".02222 0 0"/>
+  </loop_joint>
+  <gazebo reference="finger_1_link_0">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_1_link_1">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_1_link_2">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_1_link_3">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_1_fourbar_0_link_1">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_1_fourbar_0_link_2">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_1_fourbar_0_link_3">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_1_fourbar_3_link_1">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_1_fourbar_3_link_2">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_1_fourbar_3_link_3">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_1_fourbar_linkage">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <transmission name="finger_1_fourbar_0_joint_0_trans" type="SimpleTransmission">
+    <actuator name="finger_1_control_torque"/>
+    <joint name="finger_1_fourbar_0_joint_0"/>
+    <mechanicalReduction>1</mechanicalReduction>
+  </transmission>
+  <link name="finger_2_link_0">
+    <visual>
+      <origin rpy="0 0 0" xyz="0.020 0 0"/>
+      <geometry>
+        <mesh filename="package://Atlas/urdf/meshes/s-model_articulated/visual/link_0.STL"/>
+      </geometry>
+      <material name="black">
+        <color rgba=".2 .2 .2 1"/>
+      </material>
+    </visual>
+    <!--<collision>	
+				<origin xyz="-0.0455 0.0414 0.036" rpy="0 3.1416 -1.57"/>
+				<geometry>
+					<mesh filename="package://Atlas/urdf/meshes/s-model_articulated/collision/link_0.STL" />
+				</geometry>
+				<material name="yellow">
+					<color rgba="0 1 1 1"/>
+				</material>
+			</collision>-->
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.01" ixy="-0.00002" ixz="0.00001" iyy="0.0008" iyz="0" izz="0.001"/>
+    </inertial>
+  </link>
+  <link name="finger_2_link_1">
+    <visual>
+      <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+      <geometry>
+        <mesh filename="package://Atlas/urdf/meshes/s-model_articulated/visual/link_1.STL"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.52" xyz="0.03 -0.0145 0"/>
+      <geometry>
+        <box size=".028 .01 .03"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.15"/>
+      <inertia ixx="0.001859" ixy="-0.000376" ixz="0.000028" iyy="0.012756" iyz="0" izz="0.0024"/>
+    </inertial>
+  </link>
+  <link name="finger_2_link_2">
+    <!--
+			NOTE: This link has an offset in the z-direction based on the models that were
+			used to generate grap-it poses.  With the offset below the model appears visually
+			correct, but it is suspect.
+			-->
+    <visual>
+      <origin rpy="0 0 0" xyz="0.039 0 0.0075"/>
+      <geometry>
+        <mesh filename="package://Atlas/urdf/meshes/s-model_articulated/visual/link_2.STL"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.02 0.0025 -0.0005"/>
+      <geometry>
+        <box size=".018 .01 .03"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.15"/>
+      <inertia ixx="0.001859" ixy="-0.000376" ixz="0.000028" iyy="0.012756" iyz="0" izz="0.0024"/>
+    </inertial>
+  </link>
+  <link name="finger_2_link_3">
+    <visual>
+      <origin rpy="0 0 0.52" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://Atlas/urdf/meshes/s-model_articulated/visual/link_3.STL"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 .52" xyz="0.02 0.015 0"/>
+      <geometry>
+        <box size=".025 .01 .03"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.001239" ixy="-0.000251" ixz="0.000019" iyy="0.00085" iyz="0" izz="0.001632"/>
+    </inertial>
+  </link>
+  <link name="finger_2_fourbar_0_link_1">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".01429 0 0"/>
+      <geometry>
+        <cylinder length=".02858" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_2_fourbar_0_link_2">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".02699 0 0"/>
+      <geometry>
+        <cylinder length=".05398" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_2_fourbar_0_link_3">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".007365 0 0"/>
+      <geometry>
+        <cylinder length=".01473" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_2_fourbar_3_link_1">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".01111 0 0"/>
+      <geometry>
+        <cylinder length=".02222" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_2_fourbar_3_link_2">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".01905 0 0"/>
+      <geometry>
+        <cylinder length=".03810" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_2_fourbar_3_link_3">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".00635 0 0"/>
+      <geometry>
+        <cylinder length=".01270" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_2_fourbar_linkage">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".007618 0 0"/>
+      <geometry>
+        <cylinder length=".015236" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="finger_2_joint_0" type="revolute">
+    <parent link="finger_2_link_0"/>
+    <child link="finger_2_link_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.020 0 0"/>
+    <limit effort="100" lower="0" upper="1.055" velocity="100"/>
+  </joint>
+  <joint name="finger_2_joint_1" type="revolute">
+    <parent link="finger_2_link_1"/>
+    <child link="finger_2_link_2"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+    <limit effort="100" lower="0" upper="1.5708" velocity="100"/>
+  </joint>
+  <joint name="finger_2_joint_2" type="revolute">
+    <parent link="finger_2_link_2"/>
+    <child link="finger_2_link_3"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.039 0 0"/>
+    <limit effort="100" lower="-0.7854" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="finger_2_fourbar_0_joint_0" type="revolute">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_2_link_0"/>
+    <child link="finger_2_fourbar_0_link_1"/>
+    <origin rpy="0 0 0" xyz="0.020 0 0"/>
+    <limit effort="100" lower="-2.10" upper="-0.255" velocity="100"/>
+  </joint>
+  <joint name="finger_2_fourbar_0_joint_1" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_2_fourbar_0_link_1"/>
+    <child link="finger_2_fourbar_0_link_2"/>
+    <origin rpy="0 0 0" xyz=".02858 0 0"/>
+    <limit effort="100" lower="1.092" upper="1.940" velocity="100"/>
+  </joint>
+  <joint name="finger_2_fourbar_0_joint_2" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_2_link_1"/>
+    <child link="finger_2_fourbar_0_link_3"/>
+    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+    <limit effort="100" lower="-2.111" upper="-0.199" velocity="100"/>
+  </joint>
+  <loop_joint name="finger_2_fourbar_0_joint_3" type="continuous">
+    <axis xyz="0 0 1"/>
+    <link1 link="finger_2_fourbar_0_link_2" rpy="0 0 0" xyz=".05398 0 0"/>
+    <link2 link="finger_2_fourbar_0_link_3" rpy="0 0 0" xyz=".01473 0 0"/>
+  </loop_joint>
+  <joint name="finger_2_fourbar_3_joint_0" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_2_link_2"/>
+    <child link="finger_2_fourbar_3_link_1"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <limit effort="100" lower="-1.367" upper="-0.945" velocity="100"/>
+  </joint>
+  <joint name="finger_2_fourbar_3_joint_1" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_2_fourbar_3_link_1"/>
+    <child link="finger_2_fourbar_3_link_2"/>
+    <origin rpy="0 0 0" xyz="0.02222 0 0"/>
+    <limit effort="100" lower="1.235" upper="1.613" velocity="100"/>
+  </joint>
+  <joint name="finger_2_fourbar_3_joint_2" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_2_link_3"/>
+    <child link="finger_2_fourbar_3_link_3"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <limit effort="100" lower="-1.367" upper="-0.995" velocity="100"/>
+  </joint>
+  <loop_joint name="finger_2_fourbar_3_joint_3" type="continuous">
+    <axis xyz="0 0 1"/>
+    <link1 link="finger_2_fourbar_3_link_3" rpy="0 0 0" xyz="0.01270 0 0"/>
+    <link2 link="finger_2_fourbar_3_link_2" rpy="0 0 0" xyz="0.03810 0 0"/>
+  </loop_joint>
+  <joint name="finger_2_fourbar_linkage_joint_0" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_2_fourbar_0_link_3"/>
+    <child link="finger_2_fourbar_linkage"/>
+    <origin rpy="0 0 0" xyz=".01473 0 0"/>
+    <limit effort="100" lower="1.4" upper="1.5" velocity="100"/>
+  </joint>
+  <loop_joint name="finger_2_fourbar_linkage_joint_1" type="continuous">
+    <axis xyz="0 0 1"/>
+    <link1 link="finger_2_fourbar_linkage" rpy="0 0 0" xyz=".015236 0 0"/>
+    <link2 link="finger_2_fourbar_3_link_1" rpy="0 0 0" xyz=".02222 0 0"/>
+  </loop_joint>
+  <gazebo reference="finger_2_link_0">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_2_link_1">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_2_link_2">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_2_link_3">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_2_fourbar_0_link_1">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_2_fourbar_0_link_2">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_2_fourbar_0_link_3">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_2_fourbar_3_link_1">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_2_fourbar_3_link_2">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_2_fourbar_3_link_3">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_2_fourbar_linkage">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <transmission name="finger_2_fourbar_0_joint_0_trans" type="SimpleTransmission">
+    <actuator name="finger_2_control_torque"/>
+    <joint name="finger_2_fourbar_0_joint_0"/>
+    <mechanicalReduction>1</mechanicalReduction>
+  </transmission>
+  <link name="finger_middle_link_0">
+    <visual>
+      <origin rpy="0 0 0" xyz="0.020 0 0"/>
+      <geometry>
+        <mesh filename="package://Atlas/urdf/meshes/s-model_articulated/visual/link_0.STL"/>
+      </geometry>
+      <material name="black">
+        <color rgba=".2 .2 .2 1"/>
+      </material>
+    </visual>
+    <!--<collision>	
+				<origin xyz="-0.0455 0.0414 0.036" rpy="0 3.1416 -1.57"/>
+				<geometry>
+					<mesh filename="package://Atlas/urdf/meshes/s-model_articulated/collision/link_0.STL" />
+				</geometry>
+				<material name="yellow">
+					<color rgba="0 1 1 1"/>
+				</material>
+			</collision>-->
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.01" ixy="-0.00002" ixz="0.00001" iyy="0.0008" iyz="0" izz="0.001"/>
+    </inertial>
+  </link>
+  <link name="finger_middle_link_1">
+    <visual>
+      <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+      <geometry>
+        <mesh filename="package://Atlas/urdf/meshes/s-model_articulated/visual/link_1.STL"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.52" xyz="0.03 -0.0145 0"/>
+      <geometry>
+        <box size=".028 .01 .03"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.15"/>
+      <inertia ixx="0.001859" ixy="-0.000376" ixz="0.000028" iyy="0.012756" iyz="0" izz="0.0024"/>
+    </inertial>
+  </link>
+  <link name="finger_middle_link_2">
+    <!--
+			NOTE: This link has an offset in the z-direction based on the models that were
+			used to generate grap-it poses.  With the offset below the model appears visually
+			correct, but it is suspect.
+			-->
+    <visual>
+      <origin rpy="0 0 0" xyz="0.039 0 0.0075"/>
+      <geometry>
+        <mesh filename="package://Atlas/urdf/meshes/s-model_articulated/visual/link_2.STL"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.02 0.0025 -0.0005"/>
+      <geometry>
+        <box size=".018 .01 .03"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.15"/>
+      <inertia ixx="0.001859" ixy="-0.000376" ixz="0.000028" iyy="0.012756" iyz="0" izz="0.0024"/>
+    </inertial>
+  </link>
+  <link name="finger_middle_link_3">
+    <visual>
+      <origin rpy="0 0 0.52" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://Atlas/urdf/meshes/s-model_articulated/visual/link_3.STL"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 .52" xyz="0.02 0.015 0"/>
+      <geometry>
+        <box size=".025 .01 .03"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.001239" ixy="-0.000251" ixz="0.000019" iyy="0.00085" iyz="0" izz="0.001632"/>
+    </inertial>
+  </link>
+  <link name="finger_middle_fourbar_0_link_1">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".01429 0 0"/>
+      <geometry>
+        <cylinder length=".02858" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_middle_fourbar_0_link_2">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".02699 0 0"/>
+      <geometry>
+        <cylinder length=".05398" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_middle_fourbar_0_link_3">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".007365 0 0"/>
+      <geometry>
+        <cylinder length=".01473" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_middle_fourbar_3_link_1">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".01111 0 0"/>
+      <geometry>
+        <cylinder length=".02222" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_middle_fourbar_3_link_2">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".01905 0 0"/>
+      <geometry>
+        <cylinder length=".03810" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_middle_fourbar_3_link_3">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".00635 0 0"/>
+      <geometry>
+        <cylinder length=".01270" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="finger_middle_fourbar_linkage">
+    <inertial>
+      <mass value="0.00001"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57 0" xyz=".007618 0 0"/>
+      <geometry>
+        <cylinder length=".015236" radius="0.001"/>
+      </geometry>
+      <material>
+        <color rgba=".6353 .6353 .6353 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="finger_middle_joint_0" type="revolute">
+    <parent link="finger_middle_link_0"/>
+    <child link="finger_middle_link_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.020 0 0"/>
+    <limit effort="100" lower="0" upper="1.055" velocity="100"/>
+  </joint>
+  <joint name="finger_middle_joint_1" type="revolute">
+    <parent link="finger_middle_link_1"/>
+    <child link="finger_middle_link_2"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+    <limit effort="100" lower="0" upper="1.5708" velocity="100"/>
+  </joint>
+  <joint name="finger_middle_joint_2" type="revolute">
+    <parent link="finger_middle_link_2"/>
+    <child link="finger_middle_link_3"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.039 0 0"/>
+    <limit effort="100" lower="-0.7854" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="finger_middle_fourbar_0_joint_0" type="revolute">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_middle_link_0"/>
+    <child link="finger_middle_fourbar_0_link_1"/>
+    <origin rpy="0 0 0" xyz="0.020 0 0"/>
+    <limit effort="100" lower="-2.10" upper="-0.255" velocity="100"/>
+  </joint>
+  <joint name="finger_middle_fourbar_0_joint_1" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_middle_fourbar_0_link_1"/>
+    <child link="finger_middle_fourbar_0_link_2"/>
+    <origin rpy="0 0 0" xyz=".02858 0 0"/>
+    <limit effort="100" lower="1.092" upper="1.940" velocity="100"/>
+  </joint>
+  <joint name="finger_middle_fourbar_0_joint_2" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_middle_link_1"/>
+    <child link="finger_middle_fourbar_0_link_3"/>
+    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+    <limit effort="100" lower="-2.111" upper="-0.199" velocity="100"/>
+  </joint>
+  <loop_joint name="finger_middle_fourbar_0_joint_3" type="continuous">
+    <axis xyz="0 0 1"/>
+    <link1 link="finger_middle_fourbar_0_link_2" rpy="0 0 0" xyz=".05398 0 0"/>
+    <link2 link="finger_middle_fourbar_0_link_3" rpy="0 0 0" xyz=".01473 0 0"/>
+  </loop_joint>
+  <joint name="finger_middle_fourbar_3_joint_0" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_middle_link_2"/>
+    <child link="finger_middle_fourbar_3_link_1"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <limit effort="100" lower="-1.367" upper="-0.945" velocity="100"/>
+  </joint>
+  <joint name="finger_middle_fourbar_3_joint_1" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_middle_fourbar_3_link_1"/>
+    <child link="finger_middle_fourbar_3_link_2"/>
+    <origin rpy="0 0 0" xyz="0.02222 0 0"/>
+    <limit effort="100" lower="1.235" upper="1.613" velocity="100"/>
+  </joint>
+  <joint name="finger_middle_fourbar_3_joint_2" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_middle_link_3"/>
+    <child link="finger_middle_fourbar_3_link_3"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <limit effort="100" lower="-1.367" upper="-0.995" velocity="100"/>
+  </joint>
+  <loop_joint name="finger_middle_fourbar_3_joint_3" type="continuous">
+    <axis xyz="0 0 1"/>
+    <link1 link="finger_middle_fourbar_3_link_3" rpy="0 0 0" xyz="0.01270 0 0"/>
+    <link2 link="finger_middle_fourbar_3_link_2" rpy="0 0 0" xyz="0.03810 0 0"/>
+  </loop_joint>
+  <joint name="finger_middle_fourbar_linkage_joint_0" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="finger_middle_fourbar_0_link_3"/>
+    <child link="finger_middle_fourbar_linkage"/>
+    <origin rpy="0 0 0" xyz=".01473 0 0"/>
+    <limit effort="100" lower="1.4" upper="1.5" velocity="100"/>
+  </joint>
+  <loop_joint name="finger_middle_fourbar_linkage_joint_1" type="continuous">
+    <axis xyz="0 0 1"/>
+    <link1 link="finger_middle_fourbar_linkage" rpy="0 0 0" xyz=".015236 0 0"/>
+    <link2 link="finger_middle_fourbar_3_link_1" rpy="0 0 0" xyz=".02222 0 0"/>
+  </loop_joint>
+  <gazebo reference="finger_middle_link_0">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_middle_link_1">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_middle_link_2">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_middle_link_3">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_middle_fourbar_0_link_1">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_middle_fourbar_0_link_2">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_middle_fourbar_0_link_3">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_middle_fourbar_3_link_1">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_middle_fourbar_3_link_2">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_middle_fourbar_3_link_3">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="finger_middle_fourbar_linkage">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <transmission name="finger_middle_fourbar_0_joint_0_trans" type="SimpleTransmission">
+    <actuator name="finger_middle_control_torque"/>
+    <joint name="finger_middle_fourbar_0_joint_0"/>
+    <mechanicalReduction>1</mechanicalReduction>
+  </transmission>
+  <link name="palm">
+    <visual>
+      <geometry>
+        <mesh filename="package://Atlas/urdf/meshes/s-model_articulated/visual/palm.STL"/>
+      </geometry>
+      <material name="black">
+        <color rgba=".2 .2 .2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 .046 0"/>
+      <geometry>
+        <box size=".1 .01 .1"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </collision>
+  </link>
+  <inertial>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <mass value="1.3"/>
+    <inertia ixx="0.006012" ixy="0.000079" ixz="-0.00024" iyy="0.012892" iyz="0" izz="0.002435"/>
+  </inertial>
+  <joint name="palm_finger_1_joint" type="fixed">
+    <parent link="palm"/>
+    <child link="finger_1_link_0"/>
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 3.1416 -1.57" xyz="-0.0455 0.0214 0.036"/>
+    <!--<limit lower="-0.16" upper="0.25" effort="100" velocity="100"/>-->
+  </joint>
+  <joint name="palm_finger_2_joint" type="fixed">
+    <parent link="palm"/>
+    <child link="finger_2_link_0"/>
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 3.1416 -1.57" xyz="-0.0455 0.0214 -0.036"/>
+    <!--<limit lower="-0.25" upper="0.16" effort="100" velocity="100"/>-->
+  </joint>
+  <joint name="palm_finger_middle_joint" type="fixed">
+    <parent link="palm"/>
+    <child link="finger_middle_link_0"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 1.57" xyz="0.0455 0.0214 0"/>
+  </joint>
+  <gazebo reference="palm">
+    <material>Gazebo/FlatBlack</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <joint name="robotiq_hand_joint" type="fixed">
+    <origin rpy="0.0 0.0 -1.57079" xyz="0.11904 0 0"/>
+    <parent link="link_6"/>
+    <child link="palm"/>
+  </joint>
+</robot>
+
diff --git a/util/rospack.m b/util/rospack.m
index c2fb0ed95978..5ea85cb36d3c 100644
--- a/util/rospack.m
+++ b/util/rospack.m
@@ -27,6 +27,8 @@
   end
   packages{end+1} = 'Atlas';
   package_paths{end+1} = [getDrakePath,'/examples/Atlas'];
+  packages{end+1} = 'IRB140';
+  package_paths{end+1} = [getDrakePath,'/examples/IRB140'];
 end
 
 if strcmp(package,'-list')