diff --git a/CMakeLists.txt b/CMakeLists.txt
index 08893d5..f2ec576 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,6 +7,7 @@ project(kair_algorithms)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
+
 find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   rospy
@@ -14,12 +15,15 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   control_msgs
   trajectory_msgs
+  gazebo_msgs
+  message_generation
+  tf
 )
 
-add_message_files( DIRECTORY msg
-FILES
-GoalObs.msg
-)
+# add_message_files( DIRECTORY msg
+# FILES
+# GoalObs.msg
+# )
 
 
 generate_messages(
@@ -34,7 +38,6 @@ catkin_package(
   rospy
   message_generation
   message_runtime
-  cv_bridge
 )
 
 
diff --git a/package.xml b/package.xml
index 9876833..8adfc2b 100644
--- a/package.xml
+++ b/package.xml
@@ -6,4 +6,31 @@
   </description>
   <maintainer email="todo@todo.todo">TODO</maintainer>
   <license>TODO</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>tf</build_export_depend>
+  <build_export_depend>control_msgs</build_export_depend>
+  <build_export_depend>trajectory_msgs</build_export_depend>
+  <build_export_depend>gazebo_msgs</build_export_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>control_msgs</exec_depend>
+  <exec_depend>trajectory_msgs</exec_depend>
+  <exec_depend>gazebo_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>message_generation</exec_depend>
+  <exec_depend>cv_bridge</exec_depend>
 </package>
diff --git a/urdf/baxter.urdf b/urdf/baxter.urdf
deleted file mode 100644
index cbbea40..0000000
--- a/urdf/baxter.urdf
+++ /dev/null
@@ -1,1558 +0,0 @@
-<?xml version="1.0" ?>
-<!-- ============================================================================================== -->
-<!-- |                                                                                            | -->
-<!-- |    baxter.urdf is DEPRECATED. It is included here for backward compatibility.              | -->
-<!-- |    Please use baxter.urdf.xacro by running:                                                | -->
-<!-- |$ rosrun xacro xacro.py - -inorder `rospack find baxter_description`/urdf/baxter.urdf.xacro | -->
-<!-- |                                                                                            | -->
-<!-- ============================================================================================== -->
-<robot name="baxter">
-  <link name="base">
-  </link>
-  <link name="torso">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/torso/base_link.DAE"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/torso/base_link_collision.DAE"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="35.336455"/>
-      <inertia ixx="1.849155" ixy="-0.000354" ixz="-0.154188" iyy="1.662671" iyz="0.003292" izz="0.802239"/>
-    </inertial>
-  </link>
-  <link name="left_torso_itb">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="right_torso_itb">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="pedestal">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/base/PEDESTAL.DAE"/>
-      </geometry>
-      <material name="darkgray">
-        <color rgba=".2 .2 .2 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/base/pedestal_link_collision.DAE"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="60.86397744"/>
-      <inertia ixx="5.0635929" ixy="0.00103417" ixz="0.80199628" iyy="6.08689388" iyz="0.00105311" izz="4.96191932"/>
-    </inertial>
-  </link>
-  <link name="head">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 .00953"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/head/H0.DAE"/>
-      </geometry>
-      <material name="darkgray">
-        <color rgba=".2 .2 .2 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
-      <geometry>
-        <sphere radius="0.001"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.547767"/>
-      <inertia ixx="0.004641" ixy="0.000159" ixz="0.000242" iyy="0.003295" iyz="-0.001324" izz="0.003415"/>
-    </inertial>
-  </link>
-  <link name="sonar_ring">
-    <visual>
-      <origin rpy="0 0 0" xyz="-.0347 0 .00953"/>
-      <geometry>
-        <cylinder length="0.01" radius="0.085"/>
-      </geometry>
-      <material name="darkgray">
-        <color rgba=".2 .2 .2 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
-      <geometry>
-        <sphere radius="0.001"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="screen">
-    <visual>
-      <origin rpy="0 -1.57079632679 0" xyz="0 -.00953 -.0347"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/head/H1.DAE"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
-      <geometry>
-        <sphere radius="0.001"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.440171"/>
-      <inertia ixx="0.004006" ixy="0.000230" ixz="0.000002" iyy="0.002800" iyz="0.000029" izz="0.001509"/>
-    </inertial>
-  </link>
-  <link name="display">
-    <visual>
-      <origin rpy="0.2617993877991494 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.218 0.16 0.001"/>
-      </geometry>
-      <material name="black">
-        <color rgba="0 0 0 1"/>
-      </material>
-    </visual>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="head_camera">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="dummyhead1">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="collision_head_link_1">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <sphere radius="0.001"/>
-      </geometry>
-      <material name="red">
-        <color rgba="0.8 0.3 0.3 0.3"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0.0" xyz="-0.07 -0.04 0.0"/>
-      <geometry>
-        <sphere radius="0.22"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="collision_head_link_2">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <sphere radius="0.001"/>
-      </geometry>
-      <material name="red">
-        <color rgba="0.8 0.3 0.3 0.3"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.07 0.04 0.00"/>
-      <geometry>
-        <sphere radius="0.22"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <joint name="collision_head_1" type="fixed">
-    <origin rpy="0 0 0" xyz="0.11 0 0.75"/>
-    <parent link="base"/>
-    <child link="collision_head_link_1"/>
-  </joint>
-  <joint name="collision_head_2" type="fixed">
-    <origin rpy="0 0 0" xyz="0.11 0 0.75"/>
-    <parent link="base"/>
-    <child link="collision_head_link_2"/>
-  </joint>
-  <joint name="dummy" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="head"/>
-    <child link="dummyhead1"/>
-  </joint>
-  <joint name="torso_t0" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="base"/>
-    <child link="torso"/>
-    <limit effort="50000" lower="-3.01" upper="3.01" velocity="10000"/>
-  </joint>
-  <joint name="left_torso_itb_fixed" type="fixed">
-    <origin rpy="-1.57079632679 3.1415 0" xyz="-0.08897 0.15593 0.389125"/>
-    <parent link="torso"/>
-    <child link="left_torso_itb"/>
-  </joint>
-  <joint name="right_torso_itb_fixed" type="fixed">
-    <origin rpy="1.57079632679 0 0" xyz="-0.08897 -0.15593 0.389125"/>
-    <parent link="torso"/>
-    <child link="right_torso_itb"/>
-  </joint>
-  <joint name="pedestal_fixed" type="fixed">
-    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-    <parent link="torso"/>
-    <child link="pedestal"/>
-  </joint>
-  <joint name="head_pan" type="revolute">
-    <origin rpy="0 0 0" xyz="0.06 0 0.686"/>
-    <axis xyz="0 0 1"/>
-    <parent link="torso"/>
-    <child link="head"/>
-    <limit effort="50000" lower="-1.57079632679" upper="1.57079632679" velocity="10000"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="head_nod" type="fixed">
-    <origin rpy="1.75057 0 1.57079632679" xyz=".1227 0 0"/>
-    <parent link="head"/>
-    <child link="screen"/>
-  </joint>
-  <joint name="head_camera" type="fixed">
-    <origin rpy="1.75057 0 1.57079632679" xyz="0.12839 0 0.06368"/>
-    <parent link="head"/>
-    <child link="head_camera"/>
-  </joint>
-  <joint name="display_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0.0 -0.016 0.0"/>
-    <parent link="screen"/>
-    <child link="display"/>
-  </joint>
-  <joint name="sonar_s0" type="fixed">
-    <origin rpy="0 0 0" xyz="0.0947 0 .817"/>
-    <axis xyz="0 0 1"/>
-    <parent link="torso"/>
-    <child link="sonar_ring"/>
-  </joint>
-  <link name="right_arm_mount">
-    <!-- all defaults -->
- <gravity>0</gravity>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="right_upper_shoulder">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/upper_shoulder/S0.DAE"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.1361"/>
-      <geometry>
-        <cylinder length="0.2722" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.01783 0.00086 0.19127"/>
-      <mass value="5.70044"/>
-      <inertia ixx="0.04709102262" ixy="0.00012787556" ixz="0.00614870039" iyy="0.03766976455" iyz="0.00078086899" izz="0.03595988478"/>
-    </inertial>
-  </link>
-  <link name="right_lower_shoulder">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/lower_shoulder/S1.DAE"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.12" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.06845 0.00269 -0.00529"/>
-      <mass value="3.22698"/>
-      <inertia ixx="0.01175209419" ixy="-0.00030096398" ixz="0.00207675762" iyy="0.0278859752" iyz="-0.00018821993" izz="0.02078749298"/>
-    </inertial>
-  </link>
-  <link name="right_upper_elbow">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/upper_elbow/E0.DAE"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 -0.0535"/>
-      <geometry>
-        <cylinder length="0.107" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.00276 0.00132 0.18086"/>
-      <mass value="4.31272"/>
-      <inertia ixx="0.02661733557" ixy="0.00029270634" ixz="0.00392189887" iyy="0.02844355207" iyz="0.0010838933" izz="0.01248008322"/>
-    </inertial>
-  </link>
-  <link name="right_upper_elbow_visual">
- <gravity>0</gravity>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.1365"/>
-      <geometry>
-        <cylinder length="0.273" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="right_lower_elbow">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/lower_elbow/E1.DAE"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <cylinder length="0.10" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.02611 0.00159 -0.01117"/>
-      <mass value="2.07206"/>
-      <inertia ixx="0.00711582686" ixy="0.00036036173" ixz="0.0007459496" iyy="0.01318227876" iyz="-0.00019663418" izz="0.00926852064"/>
-    </inertial>
-  </link>
-  <link name="right_upper_forearm">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/upper_forearm/W0.DAE"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 -0.044"/>
-      <geometry>
-        <cylinder length="0.088" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.00168 0.0046 0.13952"/>
-      <mass value="2.24665"/>
-      <inertia ixx="0.01667742825" ixy="0.00018403705" ixz="0.00018657629" iyy="0.01675457264" iyz="-0.00064732352" izz="0.0037463115"/>
-    </inertial>
-  </link>
-  <link name="right_upper_forearm_visual">
- <gravity>0</gravity>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.136"/>
-      <geometry>
-        <cylinder length="0.272" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="right_arm_itb">
- <gravity>0</gravity>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="right_lower_forearm">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/lower_forearm/W1.DAE"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.10" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.06041 0.00697 0.006"/>
-      <mass value="1.60979"/>
-      <inertia ixx="0.00387607152" ixy="-0.00044384784" ixz="-0.00021115038" iyy="0.00700537914" iyz="0.00015348067" izz="0.0055275524"/>
-    </inertial>
-  </link>
-  <link name="right_wrist">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/wrist/W2.DAE"/>
-      </geometry>
-      <material name="lightgrey">
-        <color rgba=".1 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.165" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.00198 0.00125 0.01855"/>
-      <mass value="0.35093"/>
-      <inertia ixx="0.00025289155" ixy="0.00000575311" ixz="-0.00000159345" iyy="0.0002688601" iyz="-0.00000519818" izz="0.0003074118"/>
-    </inertial>
-  </link>
-  <link name="right_hand">
- <gravity>0</gravity>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 -0.0232"/>
-      <geometry>
-        <cylinder length="0.0464" radius="0.04"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.01093 0.00042 -0.01532"/>
-      <mass value="0.19125"/>
-      <inertia ixx="0.00017588" ixy="0.00000147073" ixz="0.0000243633" iyy="0.00021166377" iyz="0.00000172689" izz="0.00023745397"/>
-    </inertial>
-  </link>
-  <link name="right_hand_camera">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.01" radius="0.02"/>
-      </geometry>
-      <material name="blue">
-        <color rgba="0 0 1 0.8"/>
-      </material>
-    </visual>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="right_hand_camera_axis">
- <gravity>0</gravity>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="right_hand_range">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.005 .02 .005"/>
-      </geometry>
-      <material name="blue">
-        <color rgba="0 0 1 0.8"/>
-      </material>
-    </visual>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="right_hand_accelerometer">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.01 0.01 0.01"/>
-      </geometry>
-      <material name="black">
-        <color rgba="0 0 0 1"/>
-      </material>
-    </visual>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="right_gripper_base">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 -0.02"/>
-      <geometry>
-        <box size="0.025 0.075 0.035"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="right_gripper">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.01 0.01 0.01"/>
-      </geometry>
-      <material name="black">
-        <color rgba="0 0 0 1"/>
-      </material>
-    </visual>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <joint name="right_torso_arm_mount" type="fixed">
-    <origin rpy="0 0 -0.7854" xyz="0.024645 -0.219645 0.118588"/>
-    <parent link="torso"/>
-    <child link="right_arm_mount"/>
-  </joint>
-  <joint name="right_s0" type="revolute">
-    <origin rpy="0 0 0" xyz="0.055695 0 0.011038"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_arm_mount"/>
-    <child link="right_upper_shoulder"/>
-    <limit effort="50.0" lower="-1.70167993878" upper="1.70167993878" velocity="1.5"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="right_s1" type="revolute">
-    <origin rpy="-1.57079632679 0 0" xyz="0.069 0 0.27035"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_upper_shoulder"/>
-    <child link="right_lower_shoulder"/>
-    <limit effort="100.0" lower="-2.147" upper="1.047" velocity="1.5"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="right_e0" type="revolute">
-    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.102 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_lower_shoulder"/>
-    <child link="right_upper_elbow"/>
-    <limit effort="50.0" lower="-3.05417993878" upper="3.05417993878" velocity="1.5"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="right_e0_fixed" type="fixed">
-    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.107 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_lower_shoulder"/>
-    <child link="right_upper_elbow_visual"/>
-  </joint>
-  <joint name="right_e1" type="revolute">
-    <origin rpy="-1.57079632679 -1.57079632679 0" xyz="0.069 0 0.26242"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_upper_elbow"/>
-    <child link="right_lower_elbow"/>
-    <limit effort="50.0" lower="-0.05" upper="2.618" velocity="1.5"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="right_w0" type="revolute">
-    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.10359 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_lower_elbow"/>
-    <child link="right_upper_forearm"/>
-    <limit effort="15.0" lower="-3.059" upper="3.059" velocity="4.0"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="right_w0_fixed" type="fixed">
-    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.088 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_lower_elbow"/>
-    <child link="right_upper_forearm_visual"/>
-  </joint>
-  <joint name="right_w0_to_itb_fixed" type="fixed">
-    <origin rpy="-1.57079632679 0 1.57079632679" xyz="-0.0565 0 0.12"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_upper_forearm"/>
-    <child link="right_arm_itb"/>
-  </joint>
-  <joint name="right_w1" type="revolute">
-    <origin rpy="-1.57079632679 -1.57079632679 0" xyz="0.01 0 0.2707"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_upper_forearm"/>
-    <child link="right_lower_forearm"/>
-    <limit effort="15.0" lower="-1.57079632679" upper="2.094" velocity="4.0"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="right_w2" type="revolute">
-    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.115975 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_lower_forearm"/>
-    <child link="right_wrist"/>
-    <limit effort="15.0" lower="-3.059" upper="3.059" velocity="4.0"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="right_hand" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.11355"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_wrist"/>
-    <child link="right_hand"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="right_hand_camera" type="fixed">
-    <origin rpy="0 0 -1.57079633" xyz="0.03825 0.012 0.015355"/>
-    <parent link="right_hand"/>
-    <child link="right_hand_camera"/>
-  </joint>
-  <joint name="right_hand_camera_axis" type="fixed">
-    <origin rpy="0 0 0" xyz="0.03825 0.012 0.015355"/>
-    <parent link="right_hand"/>
-    <child link="right_hand_camera_axis"/>
-  </joint>
-  <joint name="right_hand_range" type="fixed">
-    <origin rpy="0 -1.57079632679 -1.57079632679" xyz="0.032 -0.020245 0.0288"/>
-    <parent link="right_hand"/>
-    <child link="right_hand_range"/>
-  </joint>
-  <joint name="right_hand_accelerometer" type="fixed">
-    <origin rpy="0 0 0" xyz="0.00198 0.000133 -0.0146"/>
-    <parent link="right_hand"/>
-    <child link="right_hand_accelerometer"/>
-  </joint>
-  <joint name="right_gripper_base" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="right_hand"/>
-    <child link="right_gripper_base"/>
-  </joint>
-  <joint name="right_endpoint" type="fixed">
-    <origin rpy="0 0 0" xyz="0.0 0 0.025"/>
-    <parent link="right_gripper_base"/>
-    <child link="right_gripper"/>
-  </joint>
-  <link name="left_arm_mount">
-    <!-- all defaults -->
- <gravity>0</gravity>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="left_upper_shoulder">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/upper_shoulder/S0.DAE"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.1361"/>
-      <geometry>
-        <cylinder length="0.2722" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.01783 0.00086 0.19127"/>
-      <mass value="5.70044"/>
-      <inertia ixx="0.04709102262" ixy="0.00012787556" ixz="0.00614870039" iyy="0.03766976455" iyz="0.00078086899" izz="0.03595988478"/>
-    </inertial>
-  </link>
-  <link name="left_lower_shoulder">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/lower_shoulder/S1.DAE"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.12" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.06845 0.00269 -0.00529"/>
-      <mass value="3.22698"/>
-      <inertia ixx="0.01175209419" ixy="-0.00030096398" ixz="0.00207675762" iyy="0.0278859752" iyz="-0.00018821993" izz="0.02078749298"/>
-    </inertial>
-  </link>
-  <link name="left_upper_elbow">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/upper_elbow/E0.DAE"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 -0.0535"/>
-      <geometry>
-        <cylinder length="0.107" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.00276 0.00132 0.18086"/>
-      <mass value="4.31272"/>
-      <inertia ixx="0.02661733557" ixy="0.00029270634" ixz="0.00392189887" iyy="0.02844355207" iyz="0.0010838933" izz="0.01248008322"/>
-    </inertial>
-  </link>
-  <link name="left_upper_elbow_visual">
- <gravity>0</gravity>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.1365"/>
-      <geometry>
-        <cylinder length="0.273" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="left_lower_elbow">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/lower_elbow/E1.DAE"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <cylinder length="0.10" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.02611 0.00159 -0.01117"/>
-      <mass value="2.07206"/>
-      <inertia ixx="0.00711582686" ixy="0.00036036173" ixz="0.0007459496" iyy="0.01318227876" iyz="-0.00019663418" izz="0.00926852064"/>
-    </inertial>
-  </link>
-  <link name="left_upper_forearm">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/upper_forearm/W0.DAE"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 -0.044"/>
-      <geometry>
-        <cylinder length="0.088" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.00168 0.0046 0.13952"/>
-      <mass value="2.24665"/>
-      <inertia ixx="0.01667742825" ixy="0.00018403705" ixz="0.00018657629" iyy="0.01675457264" iyz="-0.00064732352" izz="0.0037463115"/>
-    </inertial>
-  </link>
-  <link name="left_upper_forearm_visual">
- <gravity>0</gravity>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.136"/>
-      <geometry>
-        <cylinder length="0.272" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="left_arm_itb">
- <gravity>0</gravity>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="left_lower_forearm">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/lower_forearm/W1.DAE"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.10" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.06041 0.00697 0.006"/>
-      <mass value="1.60979"/>
-      <inertia ixx="0.00387607152" ixy="-0.00044384784" ixz="-0.00021115038" iyy="0.00700537914" iyz="0.00015348067" izz="0.0055275524"/>
-    </inertial>
-  </link>
-  <link name="left_wrist">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://baxter_description/meshes/wrist/W2.DAE"/>
-      </geometry>
-      <material name="lightgrey">
-        <color rgba=".1 .1 .1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.165" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.00198 0.00125 0.01855"/>
-      <mass value="0.35093"/>
-      <inertia ixx="0.00025289155" ixy="0.00000575311" ixz="-0.00000159345" iyy="0.0002688601" iyz="-0.00000519818" izz="0.0003074118"/>
-    </inertial>
-  </link>
-  <link name="left_hand">
- <gravity>0</gravity>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 -0.0232"/>
-      <geometry>
-        <cylinder length="0.0464" radius="0.04"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.01093 0.00042 -0.01532"/>
-      <mass value="0.19125"/>
-      <inertia ixx="0.00017588" ixy="0.00000147073" ixz="0.0000243633" iyy="0.00021166377" iyz="0.00000172689" izz="0.00023745397"/>
-    </inertial>
-  </link>
-  <link name="left_hand_camera">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.01" radius="0.02"/>
-      </geometry>
-      <material name="blue">
-        <color rgba="0 0 1 0.8"/>
-      </material>
-    </visual>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="left_hand_camera_axis">
- <gravity>0</gravity>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="left_hand_range">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.005 .02 .005"/>
-      </geometry>
-      <material name="blue">
-        <color rgba="0 0 1 0.8"/>
-      </material>
-    </visual>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="left_hand_accelerometer">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.01 0.01 0.01"/>
-      </geometry>
-      <material name="black">
-        <color rgba="0 0 0 1"/>
-      </material>
-    </visual>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="left_gripper_base">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 -0.02"/>
-      <geometry>
-        <box size="0.025 0.075 0.035"/>
-      </geometry>
-      <material name="darkred">
-        <color rgba=".5 .1 .1 1"/>
-      </material>
-    </visual>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="left_gripper">
- <gravity>0</gravity>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.01 0.01 0.01"/>
-      </geometry>
-      <material name="black">
-        <color rgba="0 0 0 1"/>
-      </material>
-    </visual>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <joint name="left_torso_arm_mount" type="fixed">
-    <origin rpy="0 0 0.7854" xyz="0.024645 0.219645 0.118588"/>
-    <parent link="torso"/>
-    <child link="left_arm_mount"/>
-  </joint>
-  <joint name="left_s0" type="revolute">
-    <origin rpy="0 0 0" xyz="0.055695 0 0.011038"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_arm_mount"/>
-    <child link="left_upper_shoulder"/>
-    <limit effort="50.0" lower="-1.70167993878" upper="1.70167993878" velocity="1.5"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="left_s1" type="revolute">
-    <origin rpy="-1.57079632679 0 0" xyz="0.069 0 0.27035"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_upper_shoulder"/>
-    <child link="left_lower_shoulder"/>
-    <limit effort="100.0" lower="-2.147" upper="1.047" velocity="1.5"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="left_e0" type="revolute">
-    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.102 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_lower_shoulder"/>
-    <child link="left_upper_elbow"/>
-    <limit effort="50.0" lower="-3.05417993878" upper="3.05417993878" velocity="1.5"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="left_e0_fixed" type="fixed">
-    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.107 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_lower_shoulder"/>
-    <child link="left_upper_elbow_visual"/>
-  </joint>
-  <joint name="left_e1" type="revolute">
-    <origin rpy="-1.57079632679 -1.57079632679 0" xyz="0.069 0 0.26242"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_upper_elbow"/>
-    <child link="left_lower_elbow"/>
-    <limit effort="50.0" lower="-0.05" upper="2.618" velocity="1.5"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="left_w0" type="revolute">
-    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.10359 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_lower_elbow"/>
-    <child link="left_upper_forearm"/>
-    <limit effort="15.0" lower="-3.059" upper="3.059" velocity="4.0"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="left_w0_fixed" type="fixed">
-    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.088 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_lower_elbow"/>
-    <child link="left_upper_forearm_visual"/>
-  </joint>
-  <joint name="left_w0_to_itb_fixed" type="fixed">
-    <origin rpy="-1.57079632679 0 1.57079632679" xyz="-0.0565 0 0.12"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_upper_forearm"/>
-    <child link="left_arm_itb"/>
-  </joint>
-  <joint name="left_w1" type="revolute">
-    <origin rpy="-1.57079632679 -1.57079632679 0" xyz="0.01 0 0.2707"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_upper_forearm"/>
-    <child link="left_lower_forearm"/>
-    <limit effort="15.0" lower="-1.57079632679" upper="2.094" velocity="4.0"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="left_w2" type="revolute">
-    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.115975 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_lower_forearm"/>
-    <child link="left_wrist"/>
-    <limit effort="15.0" lower="-3.059" upper="3.059" velocity="4.0"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="left_hand" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.11355"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_wrist"/>
-    <child link="left_hand"/>
-    <dynamics damping="0.7" friction="0.0"/>
-  </joint>
-  <joint name="left_hand_camera" type="fixed">
-    <origin rpy="0 0 -1.57079633" xyz="0.03825 0.012 0.015355"/>
-    <parent link="left_hand"/>
-    <child link="left_hand_camera"/>
-  </joint>
-  <joint name="left_hand_camera_axis" type="fixed">
-    <origin rpy="0 0 0" xyz="0.03825 0.012 0.015355"/>
-    <parent link="left_hand"/>
-    <child link="left_hand_camera_axis"/>
-  </joint>
-  <joint name="left_hand_range" type="fixed">
-    <origin rpy="0 -1.57079632679 -1.57079632679" xyz="0.032 -0.020245 0.0288"/>
-    <parent link="left_hand"/>
-    <child link="left_hand_range"/>
-  </joint>
-  <joint name="left_hand_accelerometer" type="fixed">
-    <origin rpy="0 0 0" xyz="0.00198 0.000133 -0.0146"/>
-    <parent link="left_hand"/>
-    <child link="left_hand_accelerometer"/>
-  </joint>
-  <joint name="left_gripper_base" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="left_hand"/>
-    <child link="left_gripper_base"/>
-  </joint>
-  <joint name="left_endpoint" type="fixed">
-    <origin rpy="0 0 0" xyz="0.0 0 0.025"/>
-    <parent link="left_gripper_base"/>
-    <child link="left_gripper"/>
-  </joint>
-
-  <!-- Transmissions -->
-
-  <!-- Right Arm -->
-  <transmission name="right_tran1">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_s0">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor1">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="right_tran2">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_s1">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor2">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="right_tran3">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_e0">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor3">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="right_tran4">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_e1">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor4">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="right_tran5">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_w0">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor5">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="right_tran6">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_w1">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor6">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="right_tran7">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_w2">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor7">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-
-  <!-- Left Arm -->
-  <transmission name="left_tran1">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="left_s0">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="left_motor1">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="left_tran2">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="left_s1">      
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="left_motor2">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="left_tran3">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="left_e0">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="left_motor3">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="left_tran4">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="left_e1">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="left_motor4">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="left_tran5">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="left_w0">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="left_motor5">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="left_tran6">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="left_w1">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="left_motor6">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="left_tran7">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="left_w2">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="left_motor7">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-
-  <!-- Head -->
-  <transmission name="head1">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="head_pan">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="head_motor1">
-      <hardwareInterface>EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-
-  <!-- ros_control plugin -->
-  <gazebo>
-    <plugin name="baxter_ros_control" filename="libbaxter_gazebo_ros_control.so">
-      <robotNamespace>/robot</robotNamespace>
-    </plugin>
-  </gazebo>
-
-  <!-- Gazebo-Specific Link Properties -->
-  <gazebo reference="torso">
-    <selfCollide>true</selfCollide>
-  </gazebo>
-  <gazebo reference="head_pan">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="right_s0">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="right_s1">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="right_e0">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="right_e1">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="right_w0">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="right_w1">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="right_w2">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="left_s0">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="left_s1">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="left_e0">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="left_e1">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="left_w0">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="left_w1">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="left_w2">
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <gazebo reference="head_camera">
-    <sensor name="head_camera" type="camera">
-      <update_rate>30.0</update_rate>
-      <camera name="head_camera">
-        <pose>0.0 0.21 0.0 0.0 -0.8 -1.570796327 </pose>
-        <horizontal_fov>1.3962634</horizontal_fov>
-        <image>
-          <width>800</width>
-          <height>800</height>
-          <format>R8G8B8</format>
-        </image>
-        <clip>
-          <near>0.02</near>
-          <far>300</far>
-        </clip>
-        <noise>
-          <type>gaussian</type>
-          <mean>0.0</mean>
-          <stddev>0.007</stddev>
-        </noise>
-      </camera>
-      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>0.0</updateRate>
-        <cameraName>head_camera</cameraName>
-        <imageTopicName>image</imageTopicName>
-        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-        <frameName>head_camera</frameName>
-        <hackBaseline>0.07</hackBaseline>
-        <distortionK1>0.0</distortionK1>
-        <distortionK2>0.0</distortionK2>
-        <distortionK3>0.0</distortionK3>
-        <distortionT1>0.0</distortionT1>
-        <distortionT2>0.0</distortionT2>
-        <robotNamespace>/cameras</robotNamespace>
-      </plugin>
-    </sensor>
-  </gazebo>
-  <gazebo reference="right_hand_camera">
-    <sensor name="right_hand_camera" type="camera">
-      <update_rate>30.0</update_rate>
-      <camera name="right_hand_camera">
-        <pose>0.0 0.0 0.0 0.0 -1.570796327 1.570796327 </pose>
-        <horizontal_fov>1.3962634</horizontal_fov>
-        <image>
-          <width>800</width>
-          <height>800</height>
-          <format>R8G8B8</format>
-        </image>
-        <clip>
-          <near>0.02</near>
-          <far>300</far>
-        </clip>
-        <noise>
-          <type>gaussian</type>
-          <mean>0.0</mean>
-          <stddev>0.007</stddev>
-        </noise>
-      </camera>
-      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>0.0</updateRate>
-        <cameraName>right_hand_camera</cameraName>
-        <imageTopicName>image</imageTopicName>
-        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-        <frameName>right_hand_camera</frameName>
-        <hackBaseline>0.07</hackBaseline>
-        <distortionK1>0.0</distortionK1>
-        <distortionK2>0.0</distortionK2>
-        <distortionK3>0.0</distortionK3>
-        <distortionT1>0.0</distortionT1>
-        <distortionT2>0.0</distortionT2>
-        <robotNamespace>/cameras</robotNamespace>
-      </plugin>
-    </sensor>
-  </gazebo>
-  <gazebo reference="left_hand_camera">
-    <sensor name="left_hand_camera" type="camera">
-      <update_rate>30.0</update_rate>
-      <camera name="left_hand_camera">
-        <pose>0.0 0.0 0.0 0.0 -1.570796327 1.570796327 </pose>
-        <horizontal_fov>1.3962634</horizontal_fov>
-        <image>
-          <width>800</width>
-          <height>800</height>
-          <format>R8G8B8</format>
-        </image>
-        <clip>
-          <near>0.02</near>
-          <far>300</far>
-        </clip>
-        <noise>
-          <type>gaussian</type>
-          <mean>0.0</mean>
-          <stddev>0.007</stddev>
-        </noise>
-      </camera>
-      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>0.0</updateRate>
-        <cameraName>left_hand_camera</cameraName>
-        <imageTopicName>image</imageTopicName>
-        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-        <frameName>left_hand_camera</frameName>
-        <hackBaseline>0.07</hackBaseline>
-        <distortionK1>0.0</distortionK1>
-        <distortionK2>0.0</distortionK2>
-        <distortionK3>0.0</distortionK3>
-        <distortionT1>0.0</distortionT1>
-        <distortionT2>0.0</distortionT2>
-        <robotNamespace>/cameras</robotNamespace>
-      </plugin>
-    </sensor>
-  </gazebo>
-  <gazebo reference="display">
-    <visual>
-      <plugin name="screen_video_controller" filename="libgazebo_ros_video.so">
-        <height>600</height>
-        <width>1024</width>
-        <topicName>/robot/xdisplay</topicName>
-      </plugin>
-    </visual>
-  </gazebo>
-  <gazebo reference="sonar_ring">
-    <sensor name="sonar" type="ray">
-      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-      <ray>
-        <scan>
-          <horizontal>
-            <samples>12</samples>
-            <resolution>1.0</resolution>
-            <min_angle>-3.14</min_angle>
-            <max_angle>3.14</max_angle>
-          </horizontal>
-          <vertical>
-            <samples>2</samples>
-            <resolution>1.0</resolution>
-            <min_angle>-0.001</min_angle>
-            <max_angle>0</max_angle>
-          </vertical>
-        </scan>
-        <range>
-          <min>0.05</min>
-          <max>50.0</max>
-        </range>
-      </ray>
-      <plugin name="sonar_plugin" filename="libgazebo_ros_block_laser.so">
-        <gaussianNoise>0.00</gaussianNoise>
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <topicName>/robot/sonar/head_sonar/state</topicName>
-        <frameName>sonar_ring</frameName>
-      </plugin>
-      <always_on>true</always_on>
-      <update_rate>100.0</update_rate>
-    </sensor>
-  </gazebo>
-  <gazebo reference="right_hand_range">
-    <sensor name="ir_right" type="ray">
-      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-      <ray>
-        <scan>
-          <horizontal>
-            <samples>1</samples>
-            <resolution>1.0</resolution>
-            <min_angle>-0.5</min_angle>
-            <max_angle>0.5</max_angle>
-          </horizontal>
-        </scan>
-        <range>
-          <min>0.004</min>
-          <max>0.4</max>
-        </range>
-      </ray>
-      <plugin name="plugin_3" filename="libgazebo_ros_laser.so">
-        <gaussianNoise>0.005</gaussianNoise>
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100</updateRate>
-        <topicName>/sim/laserscan/right_hand_range/state</topicName>
-        <frameName>right_hand_range</frameName>
-      </plugin>
-      <always_on>true</always_on>
-      <update_rate>100.0</update_rate>
-    </sensor>
-  </gazebo>
-  <gazebo reference="left_hand_range">
-    <sensor name="ir_left" type="ray">
-      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-      <ray>
-        <scan>
-          <horizontal>
-            <samples>1</samples>
-            <resolution>1.0</resolution>
-            <min_angle>-0.5</min_angle>
-            <max_angle>0.5</max_angle>
-          </horizontal>
-        </scan>
-        <range>
-          <min>0.004</min>
-          <max>0.4</max>
-        </range>
-      </ray>
-      <plugin name="plugin_4" filename="libgazebo_ros_laser.so">
-        <gaussianNoise>0.005</gaussianNoise>
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100</updateRate>
-        <topicName>/sim/laserscan/left_hand_range/state</topicName>
-        <frameName>left_hand_range</frameName>
-      </plugin>
-      <always_on>true</always_on>
-      <update_rate>100.0</update_rate>
-    </sensor>
-  </gazebo>
-</robot>
diff --git a/urdf/block/model.urdf b/urdf/block/model.urdf
new file mode 100755
index 0000000..ce368af
--- /dev/null
+++ b/urdf/block/model.urdf
@@ -0,0 +1,29 @@
+<robot name="block">
+  <link name="block">
+    <inertial>
+      <origin xyz="0.025 0.025 0.025" />
+      <mass value="0.5" />
+      <!-- (1/12)*mass*(h^2 + d^2)  (1/12)*mass*(w^2 + d^2) (1/12)*mass*(w^2 + h^2) -->
+      <!-- (1/12)*0.5*(0.045^2 + 0.045^2)  (1/12)*0.5*(0.045^2 + 0.045^2) (1/12)*0.5*(0.045^2 + 0.045^2) -->
+      <inertia  ixx="0.00016874999" ixy="0.0"  ixz="0.0"  iyy="0.00016874999"  iyz="0.0"  izz="0.00016874999" />
+
+    </inertial>
+    <visual>
+      <origin xyz="0.025 0.025 0.025"/>
+      <geometry>
+        <box size="0.045 0.045 0.045" />
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.025 0.025 0.025"/>
+      <geometry>
+        <box size="0.045 0.045 0.045" />
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="block">
+    <material>Gazebo/Red</material>
+     <mu1>1000</mu1>
+     <mu2>1000</mu2>
+  </gazebo>
+</robot>
diff --git a/urdf/connector_plate/connector_plate.xacro b/urdf/connector_plate/connector_plate.xacro
deleted file mode 100644
index 5b3173c..0000000
--- a/urdf/connector_plate/connector_plate.xacro
+++ /dev/null
@@ -1,44 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rethink_connector_plate">
-  <xacro:macro name="rethink_connector_plate" params="side gazebo">
-  <xacro:property name="rethink_connector_plate_length" value="0.0151" scope="global"/>
-  <link name="${side}_connector_plate_base">
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="0.0" />
-      <inertia ixx="6.9922E-05"  ixy="6.1026E-08"  ixz="-1.7042E-06"
-               iyy="6.8188E-05"  iyz="-1.6118E-08" izz="0.00012392" />
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://intera_tools_description/meshes/connector_plate/connector_plate.STL" />
-      </geometry>
-      <material name="black">
-        <color rgba="0.05 0.05 0.05 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0.0135" rpy="0 0 0" />
-        <geometry>
-          <cylinder length="${rethink_connector_plate_length*2.0}" radius="0.04"/>
-        </geometry>
-        <material name="darkred">
-          <color rgba=".5 .1 .1 1"/>
-        </material>
-    </collision>
-  </link>
-  <xacro:if value="${gazebo}">
-    <gazebo reference="${side}_connector_plate_base">
-      <material>Gazebo/FlatBlack</material>
-    </gazebo>
-  </xacro:if>
-  <link name="${side}_connector_plate_mount" />
-  <joint name="${side}_connector_plate_mount_joint" type="fixed">
-    <origin xyz="0 0 ${rethink_connector_plate_length+0.0025}" rpy="0 0 0" />
-    <parent link="${side}_connector_plate_base"  />
-    <child  link="${side}_connector_plate_mount" />
-  </joint>
-
-  </xacro:macro>
-</robot>
diff --git a/urdf/controller_box/controller_box.urdf b/urdf/controller_box/controller_box.urdf
deleted file mode 100644
index 1fca569..0000000
--- a/urdf/controller_box/controller_box.urdf
+++ /dev/null
@@ -1,32 +0,0 @@
-<?xml version="1.0"?>
-<robot name="base_collision">
-  <link name="controller_box">
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.325 0 -0.38"/>
-      <geometry>
-        <box size="0.22 0.4 0.53"/>
-      </geometry>
-    </collision>
-  </link>
-
-  <joint name="controller_box_fixed" type="fixed">
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
-    <parent link="base" />
-    <child link="controller_box" />
-  </joint>
-
-  <link name="pedestal_feet">
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.1225 0 -0.758"/>
-      <geometry>
-        <box size="0.77 0.7 0.31"/>
-      </geometry>
-    </collision>
-  </link>
-
-  <joint name="pedestal_feet_fixed" type="fixed">
-    <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
-    <parent link="base" />
-    <child link="pedestal_feet" />
-  </joint>
-</robot>
diff --git a/urdf/electric_gripper/example_end_effector.urdf.xacro b/urdf/electric_gripper/example_end_effector.urdf.xacro
deleted file mode 100644
index 9092f84..0000000
--- a/urdf/electric_gripper/example_end_effector.urdf.xacro
+++ /dev/null
@@ -1,14 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="right_electric_gripper">
-  <xacro:include filename="$(find intera_tools_description)/urdf/electric_gripper/rethink_electric_gripper.xacro" />
-  <xacro:rethink_electric_gripper side="right"
-                                  l_finger="standard_narrow"
-                                  l_finger_slot="2"
-                                  l_finger_tip="basic_hard_tip"
-                                  l_finger_grasp="inner"
-                                  r_finger="standard_narrow"
-                                  r_finger_slot="2"
-                                  r_finger_tip="basic_hard_tip"
-                                  r_finger_grasp="inner"
-                                  use_connector_plate="True"/>
-</robot>
diff --git a/urdf/electric_gripper/fingers/basic_hard_tip.xacro b/urdf/electric_gripper/fingers/basic_hard_tip.xacro
deleted file mode 100644
index b47cea1..0000000
--- a/urdf/electric_gripper/fingers/basic_hard_tip.xacro
+++ /dev/null
@@ -1,34 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="basic_hard_tip">
-  <xacro:macro name="finger_tip_xacro" params="parent_link reflect grasp *joint_origin">
-    <xacro:property name="g_reflect" value="${reflect}" scope="local"/>
-    <xacro:if value="${grasp == 'outer'}">
-      <xacro:property name="g_reflect" value="${-reflect}" scope="local"/>
-    </xacro:if>
-    <link name="${parent_link}_tip">
-      <visual>
-        <origin rpy="0 0 ${pi*(g_reflect+1)/2}" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://intera_tools_description/meshes/electric_gripper/fingers/basic_hard_tip.DAE" />
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 ${pi*(g_reflect+1)/2}" xyz="0 ${-0.0045*g_reflect} -0.015"/>
-        <geometry>
-          <box size="0.016 0.0065 0.037"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 ${pi*(g_reflect+1)/2}" xyz="0 0 0"/>
-        <mass value="0.001"/>
-        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
-      </inertial>
-    </link>
-
-    <joint name="${parent_link}_tip_joint" type="fixed">
-      <xacro:insert_block name="joint_origin" />
-      <parent link="${parent_link}"/>
-      <child link="${parent_link}_tip"/>
-    </joint>
-  </xacro:macro>
-</robot>
diff --git a/urdf/electric_gripper/fingers/basic_soft_tip.xacro b/urdf/electric_gripper/fingers/basic_soft_tip.xacro
deleted file mode 100644
index a9b9afb..0000000
--- a/urdf/electric_gripper/fingers/basic_soft_tip.xacro
+++ /dev/null
@@ -1,34 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="basic_soft_tip">
-  <xacro:macro name="finger_tip_xacro" params="parent_link reflect grasp *joint_origin">
-    <xacro:property name="g_reflect" value="${reflect}" scope="local"/>
-    <xacro:if value="${grasp == 'outer'}">
-      <xacro:property name="g_reflect" value="${-reflect}" scope="local"/>
-    </xacro:if>
-    <link name="${parent_link}_tip">
-      <visual>
-        <origin rpy="0 0 ${pi*(g_reflect+1)/2}" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://intera_tools_description/meshes/electric_gripper/fingers/basic_soft_tip.DAE" />
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 ${pi*(g_reflect+1)/2}" xyz="0 ${-0.0045*g_reflect} -0.015"/>
-        <geometry>
-          <box size="0.016 0.007 0.037"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 ${pi*(g_reflect+1)/2}" xyz="0 0 0"/>
-        <mass value="0.001"/>
-        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
-      </inertial>
-    </link>
-
-    <joint name="${parent_link}_tip_joint" type="fixed">
-      <xacro:insert_block name="joint_origin" />
-      <parent link="${parent_link}"/>
-      <child link="${parent_link}_tip"/>
-    </joint>
-  </xacro:macro>
-</robot>
diff --git a/urdf/electric_gripper/fingers/extended_narrow.xacro b/urdf/electric_gripper/fingers/extended_narrow.xacro
deleted file mode 100644
index 953abeb..0000000
--- a/urdf/electric_gripper/fingers/extended_narrow.xacro
+++ /dev/null
@@ -1,58 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="extended_narrow">
-  <xacro:macro name="finger_xacro" params="gripper_side finger_side reflect finger_tip finger_grasp gazebo">
-  <xacro:property name="finger_length" value="0.1127" scope="global"/>
-  <xacro:property name="finger_width" value="0.01725" scope="local"/>
-    <link name="${gripper_side}_gripper_${finger_side}_finger">
-      <visual>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://intera_tools_description/meshes/electric_gripper/fingers/extended_narrow.DAE" />
-        </geometry>
-      </visual>
-      <xacro:property name="finger_box_height" value="0.01" scope="local"/>
-      <xacro:property name="finger_box_width" value="0.0135" scope="local"/>
-      <xacro:property name="finger_box_length" value="0.0615" scope="local"/>
-      <collision>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="0 ${finger_width*reflect} ${finger_box_length}"/>
-        <geometry>
-          <box size="${finger_box_height} ${finger_box_width} ${finger_length}"/>
-        </geometry>
-      </collision>
-      <xacro:property name="knuckle_box_height" value="0.01" scope="local"/>
-      <xacro:property name="knuckle_box_width" value="0.017" scope="local"/>
-      <xacro:property name="knuckle_box_length" value="${finger_width + 0.03275}" scope="local"/>
-      <collision>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="${reflect*-0.005} ${-0.003*reflect} 0.0083"/>
-        <geometry>
-          <box size="${knuckle_box_height} ${knuckle_box_length} ${knuckle_box_width}"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="0 0 0"/>
-        <mass value="0.001"/>
-        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
-      </inertial>
-    </link>
-
-    <xacro:if value="${gazebo}">
-      <gazebo reference="${gripper_side}_gripper_${finger_side}_finger">
-        <mu1>1000</mu1>
-        <mu2>1000</mu2>
-        <fdir1>0.0 0.0 1.0</fdir1>
-        <kp>1e5</kp>
-        <kd>1.0</kd>
-      </gazebo>
-    </xacro:if>
-
-    <xacro:property name="none" value="[none, false, null, empty]" scope="local"/>
-    <xacro:if value="${finger_tip in none}">
-      <xacro:property name="finger_tip" value="none" scope="local"/>
-    </xacro:if>
-    <xacro:include filename="$(find intera_tools_description)/urdf/electric_gripper/fingers/${finger_tip}.xacro" />
-    <xacro:finger_tip_xacro parent_link="${gripper_side}_gripper_${finger_side}_finger" reflect="${reflect}" grasp="${finger_grasp}">
-        <origin rpy="0 0 0" xyz="0.0 ${reflect*finger_width} ${finger_length}"/>
-    </xacro:finger_tip_xacro>
-
-  </xacro:macro>
-</robot>
diff --git a/urdf/electric_gripper/fingers/extended_wide.xacro b/urdf/electric_gripper/fingers/extended_wide.xacro
deleted file mode 100644
index c2f2339..0000000
--- a/urdf/electric_gripper/fingers/extended_wide.xacro
+++ /dev/null
@@ -1,58 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="wide_extended">
-  <xacro:macro name="finger_xacro" params="gripper_side finger_side reflect finger_tip finger_grasp gazebo">
-    <xacro:property name="finger_length" value="0.1127" scope="global"/>
-    <xacro:property name="finger_width" value="0.05503" scope="local"/>
-    <link name="${gripper_side}_gripper_${finger_side}_finger">
-      <visual>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://intera_tools_description/meshes/electric_gripper/fingers/extended_wide.DAE" />
-        </geometry>
-      </visual>
-     <xacro:property name="finger_box_height" value="0.01" scope="local"/>
-     <xacro:property name="finger_box_width" value="0.0135" scope="local"/>
-     <xacro:property name="finger_box_length" value="0.0615" scope="local"/>
-     <collision>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="0 ${finger_width*reflect} ${finger_box_length}"/>
-        <geometry>
-          <box size="${finger_box_height} ${finger_box_width} ${finger_length}"/>
-        </geometry>
-      </collision>
-      <xacro:property name="knuckle_box_height" value="0.01" scope="local"/>
-      <xacro:property name="knuckle_box_width" value="0.017" scope="local"/>
-      <xacro:property name="knuckle_box_length" value="${finger_width + 0.03275}" scope="local"/>
-      <collision>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="${reflect*-0.005} ${0.016*reflect} 0.0083"/>
-        <geometry>
-          <box size="${knuckle_box_height} ${knuckle_box_length} ${knuckle_box_width}"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="0 0 0"/>
-        <mass value="0.001"/>
-        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
-      </inertial>
-    </link>
-
-    <xacro:if value="${gazebo}">
-      <gazebo reference="${gripper_side}_gripper_${finger_side}_finger">
-        <mu1>1000</mu1>
-        <mu2>1000</mu2>
-        <fdir1>0.0 0.0 1.0</fdir1>
-        <kp>1e5</kp>
-        <kd>1.0</kd>
-      </gazebo>
-    </xacro:if>
-
-    <xacro:property name="none" value="[none, false, null, empty]" scope="local"/>
-    <xacro:if value="${finger_tip in none}">
-      <xacro:property name="finger_tip" value="none" scope="local"/>
-    </xacro:if>
-    <xacro:include filename="$(find intera_tools_description)/urdf/electric_gripper/fingers/${finger_tip}.xacro" />
-    <xacro:finger_tip_xacro parent_link="${gripper_side}_gripper_${finger_side}_finger" reflect="${reflect}" grasp="${finger_grasp}">
-        <origin rpy="0 0 0" xyz="0.0 ${reflect*finger_width} ${finger_length}"/>
-    </xacro:finger_tip_xacro>
-
-  </xacro:macro>
-</robot>
diff --git a/urdf/electric_gripper/fingers/half_round_tip.xacro b/urdf/electric_gripper/fingers/half_round_tip.xacro
deleted file mode 100644
index bc002fb..0000000
--- a/urdf/electric_gripper/fingers/half_round_tip.xacro
+++ /dev/null
@@ -1,34 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="half_round_tip">
-  <xacro:macro name="finger_tip_xacro" params="parent_link reflect grasp *joint_origin">
-    <xacro:property name="g_reflect" value="${reflect}" scope="local"/>
-    <xacro:if value="${grasp == 'outer'}">
-      <xacro:property name="g_reflect" value="${-reflect}" scope="local"/>
-    </xacro:if>
-    <link name="${parent_link}_tip">
-      <visual>
-        <origin rpy="0 0 ${pi*(g_reflect+1)/2}" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://intera_tools_description/meshes/electric_gripper/fingers/half_round_tip.DAE" />
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 ${pi*(g_reflect+1)/2}" xyz="0 ${-0.0045*g_reflect} -0.015"/>
-        <geometry>
-          <cylinder length="0.037" radius="0.008"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 ${pi*(g_reflect+1)/2}" xyz="0 0 0"/>
-        <mass value="0.001"/>
-        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
-      </inertial>
-    </link>
-
-    <joint name="${parent_link}_tip_joint" type="fixed">
-      <xacro:insert_block name="joint_origin" />
-      <parent link="${parent_link}"/>
-      <child link="${parent_link}_tip"/>
-    </joint>
-  </xacro:macro>
-</robot>
diff --git a/urdf/electric_gripper/fingers/none.xacro b/urdf/electric_gripper/fingers/none.xacro
deleted file mode 100644
index 5911d2a..0000000
--- a/urdf/electric_gripper/fingers/none.xacro
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="none_tip">
-  <xacro:macro name="finger_tip_xacro" params="parent_link grip reflect *joint_origin">
-    <link name="${parent_link}_tip">
-    </link>
-
-    <joint name="${parent_link}_tip_joint" type="fixed">
-      <xacro:insert_block name="joint_origin" />
-      <parent link="${parent_link}"/>
-      <child link="${parent_link}_tip"/>
-    </joint>
-  </xacro:macro>
-</robot>
diff --git a/urdf/electric_gripper/fingers/paddle_tip.xacro b/urdf/electric_gripper/fingers/paddle_tip.xacro
deleted file mode 100644
index 28ebc81..0000000
--- a/urdf/electric_gripper/fingers/paddle_tip.xacro
+++ /dev/null
@@ -1,34 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="paddle_tip">
-  <xacro:macro name="finger_tip_xacro" params="parent_link reflect grasp *joint_origin">
-   <xacro:property name="g_reflect" value="${reflect}" scope="local"/>
-   <xacro:if value="${grasp == 'outer'}">
-     <xacro:property name="g_reflect" value="${-reflect}" scope="local"/>
-   </xacro:if>
-    <link name="${parent_link}_tip">
-      <visual>
-        <origin rpy="0 0 ${pi*(g_reflect+1)/2}" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://intera_tools_description/meshes/electric_gripper/fingers/paddle_tip.DAE" />
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 ${pi*(g_reflect+1)/2}" xyz="0 ${-0.0045*g_reflect} -0.015"/>
-        <geometry>
-          <box size="0.042 0.0065 0.037"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 ${pi*(g_reflect+1)/2}" xyz="0 0 0"/>
-        <mass value="0.001"/>
-        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
-      </inertial>
-    </link>
-
-    <joint name="${parent_link}_tip_joint" type="fixed">
-      <xacro:insert_block name="joint_origin" />
-      <parent link="${parent_link}"/>
-      <child link="${parent_link}_tip"/>
-    </joint>
-  </xacro:macro>
-</robot>
diff --git a/urdf/electric_gripper/fingers/standard_narrow.xacro b/urdf/electric_gripper/fingers/standard_narrow.xacro
deleted file mode 100644
index 8b203a3..0000000
--- a/urdf/electric_gripper/fingers/standard_narrow.xacro
+++ /dev/null
@@ -1,58 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="standard_narrow">
-  <xacro:macro name="finger_xacro" params="gripper_side finger_side reflect finger_tip finger_grasp gazebo">
-  <xacro:property name="finger_length" value="0.075" scope="global"/>
-  <xacro:property name="finger_width" value="0.01725" scope="local"/>
-    <link name="${gripper_side}_gripper_${finger_side}_finger">
-      <visual>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://intera_tools_description/meshes/electric_gripper/fingers/standard_narrow.DAE" />
-        </geometry>
-      </visual>
-      <xacro:property name="finger_box_height" value="0.01" scope="local"/>
-      <xacro:property name="finger_box_width" value="0.0135" scope="local"/>
-      <xacro:property name="finger_box_length" value="0.04" scope="local"/>
-      <collision>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="0 ${finger_width*reflect} ${finger_box_length}"/>
-        <geometry>
-          <box size="${finger_box_height} ${finger_box_width} ${finger_length}"/>
-        </geometry>
-      </collision>
-      <xacro:property name="knuckle_box_height" value="0.01" scope="local"/>
-      <xacro:property name="knuckle_box_width" value="0.017" scope="local"/>
-      <xacro:property name="knuckle_box_length" value="${finger_width + 0.03275}" scope="local"/>
-      <collision>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="${reflect*-0.005} ${-0.003*reflect} 0.0083"/>
-        <geometry>
-          <box size="${knuckle_box_height} ${knuckle_box_length} ${knuckle_box_width}"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="0 0 0"/>
-        <mass value="0.001"/>
-        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
-      </inertial>
-    </link>
-
-    <xacro:if value="${gazebo}">
-      <gazebo reference="${gripper_side}_gripper_${finger_side}_finger">
-        <mu1>1000</mu1>
-        <mu2>1000</mu2>
-        <fdir1>0.0 0.0 1.0</fdir1>
-        <kp>1e5</kp>
-        <kd>1.0</kd>
-      </gazebo>
-    </xacro:if>
-
-    <xacro:property name="none" value="[none, false, null, empty]" scope="local"/>
-    <xacro:if value="${finger_tip in none}">
-      <xacro:property name="finger_tip" value="none" scope="local"/>
-    </xacro:if>
-    <xacro:include filename="$(find intera_tools_description)/urdf/electric_gripper/fingers/${finger_tip}.xacro" />
-    <xacro:finger_tip_xacro parent_link="${gripper_side}_gripper_${finger_side}_finger" reflect="${reflect}" grasp="${finger_grasp}">
-        <origin rpy="0 0 0" xyz="0.0 ${reflect*finger_width} ${finger_length}"/>
-    </xacro:finger_tip_xacro>
-
-  </xacro:macro>
-</robot>
diff --git a/urdf/electric_gripper/fingers/standard_wide.xacro b/urdf/electric_gripper/fingers/standard_wide.xacro
deleted file mode 100644
index 29ea2d5..0000000
--- a/urdf/electric_gripper/fingers/standard_wide.xacro
+++ /dev/null
@@ -1,58 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="standard_wide">
-  <xacro:macro name="finger_xacro" params="gripper_side finger_side reflect finger_tip finger_grasp gazebo">
-  <xacro:property name="finger_length" value="0.075" scope="global"/>
-  <xacro:property name="finger_width" value="0.05503" scope="local"/>
-    <link name="${gripper_side}_gripper_${finger_side}_finger">
-      <visual>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://intera_tools_description/meshes/electric_gripper/fingers/standard_wide.DAE" />
-        </geometry>
-      </visual>
-      <xacro:property name="finger_box_height" value="0.01" scope="local"/>
-      <xacro:property name="finger_box_width" value="0.0135" scope="local"/>
-      <xacro:property name="finger_box_length" value="0.04" scope="local"/>
-      <collision>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="0 ${finger_width*reflect} ${finger_box_length}"/>
-        <geometry>
-          <box size="${finger_box_height} ${finger_box_width} ${finger_length}"/>
-        </geometry>
-      </collision>
-      <xacro:property name="knuckle_box_height" value="0.01" scope="local"/>
-      <xacro:property name="knuckle_box_width" value="0.017" scope="local"/>
-      <xacro:property name="knuckle_box_length" value="${finger_width + 0.03275}" scope="local"/>
-      <collision>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="${reflect*-0.005} ${0.016*reflect} 0.0083"/>
-        <geometry>
-          <box size="${knuckle_box_height} ${knuckle_box_length} ${knuckle_box_width}"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 ${-pi/2*(reflect+1)}" xyz="0 0 0"/>
-        <mass value="0.001"/>
-        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
-      </inertial>
-    </link>
-
-    <xacro:if value="${gazebo}">
-      <gazebo reference="${gripper_side}_gripper_${finger_side}_finger">
-        <mu1>1000</mu1>
-        <mu2>1000</mu2>
-        <fdir1>0.0 0.0 1.0</fdir1>
-        <kp>1e5</kp>
-        <kd>1.0</kd>
-      </gazebo>
-    </xacro:if>
-
-    <xacro:property name="none" value="[none, false, null, empty]" scope="local"/>
-    <xacro:if value="${finger_tip in none}">
-      <xacro:property name="finger_tip" value="none" scope="local"/>
-    </xacro:if>
-    <xacro:include filename="$(find intera_tools_description)/urdf/electric_gripper/fingers/${finger_tip}.xacro" />
-    <xacro:finger_tip_xacro parent_link="${gripper_side}_gripper_${finger_side}_finger" reflect="${reflect}" grasp="${finger_grasp}">
-        <origin rpy="0 0 0" xyz="0.0 ${reflect*finger_width} ${finger_length}"/>
-    </xacro:finger_tip_xacro>
-
-  </xacro:macro>
-</robot>
diff --git a/urdf/electric_gripper/rethink_electric_gripper.xacro b/urdf/electric_gripper/rethink_electric_gripper.xacro
deleted file mode 100644
index e7ce36d..0000000
--- a/urdf/electric_gripper/rethink_electric_gripper.xacro
+++ /dev/null
@@ -1,156 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rethink_electric_gripper">
-  <xacro:macro name="rethink_electric_gripper" params="side r_finger r_finger_slot r_finger_tip r_finger_grasp l_finger l_finger_slot l_finger_tip l_finger_grasp use_connector_plate gazebo">
-    <xacro:property name="gripper_side" value="${side}" scope="local"/>
-
-    <!-- Base of whole end effector - (Connector Plate + Electric Gripper + Fingers + Tips) -->
-    <link name="${side}_gripper_base">
-      <inertial>
-        <!-- Sum Gripper Mass - for whole end effector, measured from end of plastic cuff -->
-        <mass value="0.47"/>
-        <origin rpy="0 0 0" xyz="0.0028 -0.0004 0.0343"/>
-        <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-      </inertial>
-    </link>
-
-    <!-- Electric Gripper Base Link -->
-    <link name="${side}_electric_gripper_base">
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://intera_tools_description/meshes/electric_gripper/electric_gripper_base.DAE"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="${-pi/2} ${pi} 0" xyz="0.004 0.0 0.025"/>
-        <geometry>
-          <cylinder length="0.1" radius="0.029"/>
-        </geometry>
-        <material name="darkred">
-          <color rgba=".5 .1 .1 1"/>
-        </material>
-      </collision>
-      <inertial>
-        <origin rpy="${-pi/2} ${pi} 0" xyz="0.0 0.0 0.0"/>
-        <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>
-
-   <xacro:unless value="${use_connector_plate}">
-     <xacro:property name="electric_gripper_parent_link" value="${side}_gripper_base" scope="local"/>
-     <xacro:property name="connector_plate_length" value="0.0" scope="local"/>
-   </xacro:unless>
-   <xacro:if value="${use_connector_plate}">
-     <!-- Connector Plate Joint -->
-     <joint name="${side}_connector_plate_base_joint" type="fixed">
-       <origin xyz="0 0 0.0018" rpy="0 0 0" />
-       <parent link="${side}_gripper_base"  />
-       <child  link="${side}_connector_plate_base" />
-     </joint>
-     <!-- Connector Plate Link -->
-     <xacro:include filename="$(find ddpg)/urdf/connector_plate/connector_plate.xacro" />
-     <xacro:rethink_connector_plate side="${side}" gazebo="${gazebo}"/>
-     <xacro:property name="electric_gripper_parent_link" value="${side}_connector_plate_mount" scope="local"/>
-     <xacro:property name="connector_plate_length" value="${rethink_connector_plate_length}" scope="local"/>
-   </xacro:if>
-
-   <!-- Electric Gripper Base joint -->
-    <joint name="${side}_electric_gripper_base_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="${electric_gripper_parent_link}"/>
-      <child link="${side}_electric_gripper_base"/>
-    </joint>
-
-    <!-- Left finger link -->
-    <xacro:property name="finger_length" value="0.0" scope="global"/>
-    <xacro:include filename="$(find ddpg)/urdf/electric_gripper/fingers/${l_finger}.xacro" />
-    <xacro:finger_xacro gripper_side="${gripper_side}" finger_side="l" reflect="1" finger_tip="${l_finger_tip}" finger_grasp="${l_finger_grasp}" gazebo="${gazebo}"/>
-    <xacro:property name="tip_finger_length" value="${finger_length}" scope="local"/>
-
-    <!-- Right finger link -->
-    <xacro:include filename="$(find ddpg)/urdf/electric_gripper/fingers/${r_finger}.xacro" />
-    <xacro:finger_xacro gripper_side="${gripper_side}" finger_side="r" reflect="-1" finger_tip="${r_finger_tip}" finger_grasp="${r_finger_grasp}" gazebo="${gazebo}"/>
-    <xacro:if value="${finger_length > tip_finger_length}">
-        <xacro:property name="tip_finger_length" value="${finger_length}" scope="local"/>
-    </xacro:if>
-
-    <!-- Gripper Base Joint -->
-    <xacro:property name="end_of_arm_offset" value="-0.005" scope="local"/>
-    <joint name="${side}_gripper_base_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 ${end_of_arm_offset}"/>
-      <parent link="${side}_hand"/>
-      <child link="${side}_gripper_base"/>
-    </joint>
-
-    <!-- Gripper Tip joint -->
-    <xacro:property name="electric_gripper_base_length" value="0.04552" scope="local"/>
-    <joint name="${side}_gripper_tip_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 ${(-1*end_of_arm_offset)+connector_plate_length+electric_gripper_base_length+tip_finger_length}"/>
-      <parent link="${side}_gripper_base"/>
-      <child link="${side}_gripper_tip"/>
-    </joint>
-
-    <!-- Electric Gripper Tip link -->
-    <link name="${side}_gripper_tip">
-      <inertial>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <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>
-
-    <!-- Left finger -->
-    <xacro:property name="l_finger_offset" value="${((l_finger_slot-1)*0.0095) -0.011}" scope="local"/>
-    <joint name="${gripper_side}_gripper_l_finger_joint" type="prismatic">
-      <origin rpy="0 0 0" xyz="0.003 ${l_finger_offset} ${electric_gripper_base_length}"/>
-      <axis xyz="0 1 0"/>
-      <parent link="${side}_electric_gripper_base"/>
-      <child link="${gripper_side}_gripper_l_finger"/>
-      <limit effort="20.0" lower="0.0" upper="0.020833" velocity="5.0"/>
-      <dynamics damping="100.0" friction="500.0"/>
-    </joint>
-
-    <!-- Right finger -->
-    <xacro:property name="r_finger_offset" value="${((r_finger_slot-1)*-0.0095) +0.011}" scope="local"/>
-    <joint name="${gripper_side}_gripper_r_finger_joint" type="prismatic">
-      <origin rpy="0 0 0" xyz="0.003 ${r_finger_offset} ${electric_gripper_base_length}"/>
-      <axis xyz="0 1 0"/>
-      <parent link="${side}_electric_gripper_base"/>
-      <child link="${gripper_side}_gripper_r_finger"/>
-      <limit effort="20.0" lower="-0.020833" upper="0.0" velocity="5.0"/>
-      <mimic joint="${gripper_side}_gripper_l_finger_joint" multiplier="-1.0"/>
-      <dynamics damping="100.0" friction="500.0"/>
-    </joint>
-
-    <xacro:if value="${gazebo}">
-    <transmission name="gripper_${side}1">
-      <type>transmission_interface/SimpleTransmission</type>
-      <joint name="${gripper_side}_gripper_r_finger_joint">
-        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-      </joint>
-      <actuator name="gripper_${gripper_side}1_motor1">
-        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-        <mechanicalReduction>1</mechanicalReduction>
-      </actuator>
-    </transmission>
-    <transmission name="gripper_${side}2">
-      <type>transmission_interface/SimpleTransmission</type>
-      <joint name="${gripper_side}_gripper_l_finger_joint">
-        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-      </joint>
-      <actuator name="gripper_${gripper_side}1_motor2">
-        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-        <mechanicalReduction>1</mechanicalReduction>
-      </actuator>
-    </transmission>
-    <gazebo reference="${gripper_side}_gripper_r_finger_joint">
-      <implicitSpringDamper>1</implicitSpringDamper>
-    </gazebo>
-    <gazebo reference="${gripper_side}_gripper_l_finger_joint">
-      <implicitSpringDamper>1</implicitSpringDamper>
-    </gazebo>
-    </xacro:if>
-
-  </xacro:macro>
-</robot>
diff --git a/urdf/example_end_effector.urdf.xacro b/urdf/example_end_effector.urdf.xacro
deleted file mode 100644
index 9092f84..0000000
--- a/urdf/example_end_effector.urdf.xacro
+++ /dev/null
@@ -1,14 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="right_electric_gripper">
-  <xacro:include filename="$(find intera_tools_description)/urdf/electric_gripper/rethink_electric_gripper.xacro" />
-  <xacro:rethink_electric_gripper side="right"
-                                  l_finger="standard_narrow"
-                                  l_finger_slot="2"
-                                  l_finger_tip="basic_hard_tip"
-                                  l_finger_grasp="inner"
-                                  r_finger="standard_narrow"
-                                  r_finger_slot="2"
-                                  r_finger_tip="basic_hard_tip"
-                                  r_finger_grasp="inner"
-                                  use_connector_plate="True"/>
-</robot>
diff --git a/urdf/null_gripper/example_end_effector.urdf.xacro b/urdf/null_gripper/example_end_effector.urdf.xacro
deleted file mode 100644
index da99cfe..0000000
--- a/urdf/null_gripper/example_end_effector.urdf.xacro
+++ /dev/null
@@ -1,5 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="left_null_gripper">
-  <xacro:include filename="$(find intera_tools_description)/urdf/null_gripper/null_gripper.xacro" />
-  <xacro:null_gripper side="left"/>
-</robot>
diff --git a/urdf/null_gripper/null_gripper.xacro b/urdf/null_gripper/null_gripper.xacro
deleted file mode 100644
index 3f9c25e..0000000
--- a/urdf/null_gripper/null_gripper.xacro
+++ /dev/null
@@ -1,33 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="null_gripper">
-  <xacro:macro name="null_gripper" params="side">
-  <!-- This is not a URDF, it is a URDF fragment describing a default end effector
-     to be added to the robot URDF at run time.
-  -->
-  <link name="${side}_gripper_base">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="${side}_gripper">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <joint name="${side}_gripper_base" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="${side}_hand"/>
-    <child link="${side}_gripper_base"/>
-  </joint>
-  <joint name="${side}_endpoint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="${side}_gripper_base"/>
-    <child link="${side}_gripper"/>
-  </joint>
-
-  </xacro:macro>
-</robot>
diff --git a/urdf/pneumatic_gripper/example_end_effector.urdf.xacro b/urdf/pneumatic_gripper/example_end_effector.urdf.xacro
deleted file mode 100644
index 129bd88..0000000
--- a/urdf/pneumatic_gripper/example_end_effector.urdf.xacro
+++ /dev/null
@@ -1,5 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="left_pneumatic_gripper">
-  <xacro:include filename="$(find intera_tools_description)/urdf/electric_gripper/rethink_pneumatic_gripper.xacro" />
-  <xacro:rethink_pneumatic_gripper side="left"/>
-</robot>
diff --git a/urdf/pneumatic_gripper/pneumatic_gripper_base.xacro b/urdf/pneumatic_gripper/pneumatic_gripper_base.xacro
deleted file mode 100644
index 82b59f7..0000000
--- a/urdf/pneumatic_gripper/pneumatic_gripper_base.xacro
+++ /dev/null
@@ -1,54 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="pneumatic_gripper_base">
-  <xacro:macro name="pneumatic_gripper_base" params="side">
-
-    <xacro:property name="tip_length" value="${0.03 + 0.02}" scope="local"/>
-
-    <!-- Base of end effector -->
-    <link name="${side}_gripper_base">
-      <visual>
-        <origin rpy="0 ${pi} ${pi}" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://intera_tools_description/meshes/pneumatic_gripper/pneumatic_gripper_base.DAE"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 ${pi} ${pi}" xyz="0.0 0.0 0.025"/>
-        <geometry>
-          <box size="0.044 0.078 0.038" />
-          <box length="0.08" radius="0.02"/>
-        </geometry>
-        <material name="darkred">
-          <color rgba=".5 .1 .1 1"/>
-        </material>
-      </collision>
-      <inertial>
-        <origin rpy="0 ${pi} ${pi}" xyz="0.0 0.0 0.0"/>
-        <mass value="0.1"/>
-        <inertia ixx="2e-08" ixy="0" ixz="0" iyy="3e-08" iyz="0" izz="2e-08"/>
-      </inertial>
-    </link>
-
-    <!-- Pneumatic Gripper Base Joint -->
-    <joint name="${side}_gripper_base" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0.0"/>
-      <parent link="${side}_hand"/>
-      <child link="${side}_gripper_base"/>
-    </joint>
-    <!-- Pneumatic Gripper Tip joint -->
-    <joint name="${side}_endpoint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 ${tip_length}"/>
-      <parent link="${side}_gripper_base"/>
-      <child link="${side}_gripper"/>
-    </joint>
-    <!-- Pneumatic Gripper Tip link -->
-    <link name="${side}_gripper">
-      <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <mass value="0.0001"/>
-        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0.0"/>
-      </inertial>
-    </link>
-
-  </xacro:macro>
-</robot>
diff --git a/urdf/pneumatic_gripper/rethink_pneumatic_gripper.xacro b/urdf/pneumatic_gripper/rethink_pneumatic_gripper.xacro
deleted file mode 100644
index 58f1fe4..0000000
--- a/urdf/pneumatic_gripper/rethink_pneumatic_gripper.xacro
+++ /dev/null
@@ -1,53 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rethink_pneumatic_gripper">
-  <xacro:macro name="rethink_pneumatic_gripper" params="side">
-
-    <xacro:property name="tip_length" value="${0.06 + 0.02}" scope="local"/>
-
-    <!-- Base of end effector -->
-    <link name="${side}_gripper_base">
-      <visual>
-        <origin rpy="0 ${pi} ${pi}" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://intera_tools_description/meshes/pneumatic_gripper/pneumatic_gripper_w_cup.DAE"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 ${pi} ${pi}" xyz="0.0 0.0 0.04"/>
-        <geometry>
-          <cylinder length="0.08" radius="0.02"/>
-        </geometry>
-        <material name="darkred">
-          <color rgba=".5 .1 .1 1"/>
-        </material>
-      </collision>
-      <inertial>
-        <origin rpy="0 ${pi} ${pi}" xyz="0.0 0.0 0.0"/>
-        <mass value="0.3"/>
-        <inertia ixx="2e-08" ixy="0" ixz="0" iyy="3e-08" iyz="0" izz="2e-08"/>
-      </inertial>
-    </link>
-
-    <!-- Pneumatic Gripper Base Joint -->
-    <joint name="${side}_gripper_base" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0.0"/>
-      <parent link="${side}_hand"/>
-      <child link="${side}_gripper_base"/>
-    </joint>
-    <!-- Pneumatic Gripper Tip joint -->
-    <joint name="${side}_endpoint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 ${tip_length}"/>
-      <parent link="${side}_gripper_base"/>
-      <child link="${side}_gripper"/>
-    </joint>
-    <!-- Pneumatic Gripper Tip link -->
-    <link name="${side}_gripper">
-      <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <mass value="0.0001"/>
-        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0.0"/>
-      </inertial>
-    </link>
-
-  </xacro:macro>
-</robot>
diff --git a/urdf/rethink_electric_gripper.xacro b/urdf/rethink_electric_gripper.xacro
deleted file mode 100644
index e7ce36d..0000000
--- a/urdf/rethink_electric_gripper.xacro
+++ /dev/null
@@ -1,156 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rethink_electric_gripper">
-  <xacro:macro name="rethink_electric_gripper" params="side r_finger r_finger_slot r_finger_tip r_finger_grasp l_finger l_finger_slot l_finger_tip l_finger_grasp use_connector_plate gazebo">
-    <xacro:property name="gripper_side" value="${side}" scope="local"/>
-
-    <!-- Base of whole end effector - (Connector Plate + Electric Gripper + Fingers + Tips) -->
-    <link name="${side}_gripper_base">
-      <inertial>
-        <!-- Sum Gripper Mass - for whole end effector, measured from end of plastic cuff -->
-        <mass value="0.47"/>
-        <origin rpy="0 0 0" xyz="0.0028 -0.0004 0.0343"/>
-        <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-      </inertial>
-    </link>
-
-    <!-- Electric Gripper Base Link -->
-    <link name="${side}_electric_gripper_base">
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://intera_tools_description/meshes/electric_gripper/electric_gripper_base.DAE"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="${-pi/2} ${pi} 0" xyz="0.004 0.0 0.025"/>
-        <geometry>
-          <cylinder length="0.1" radius="0.029"/>
-        </geometry>
-        <material name="darkred">
-          <color rgba=".5 .1 .1 1"/>
-        </material>
-      </collision>
-      <inertial>
-        <origin rpy="${-pi/2} ${pi} 0" xyz="0.0 0.0 0.0"/>
-        <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>
-
-   <xacro:unless value="${use_connector_plate}">
-     <xacro:property name="electric_gripper_parent_link" value="${side}_gripper_base" scope="local"/>
-     <xacro:property name="connector_plate_length" value="0.0" scope="local"/>
-   </xacro:unless>
-   <xacro:if value="${use_connector_plate}">
-     <!-- Connector Plate Joint -->
-     <joint name="${side}_connector_plate_base_joint" type="fixed">
-       <origin xyz="0 0 0.0018" rpy="0 0 0" />
-       <parent link="${side}_gripper_base"  />
-       <child  link="${side}_connector_plate_base" />
-     </joint>
-     <!-- Connector Plate Link -->
-     <xacro:include filename="$(find ddpg)/urdf/connector_plate/connector_plate.xacro" />
-     <xacro:rethink_connector_plate side="${side}" gazebo="${gazebo}"/>
-     <xacro:property name="electric_gripper_parent_link" value="${side}_connector_plate_mount" scope="local"/>
-     <xacro:property name="connector_plate_length" value="${rethink_connector_plate_length}" scope="local"/>
-   </xacro:if>
-
-   <!-- Electric Gripper Base joint -->
-    <joint name="${side}_electric_gripper_base_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="${electric_gripper_parent_link}"/>
-      <child link="${side}_electric_gripper_base"/>
-    </joint>
-
-    <!-- Left finger link -->
-    <xacro:property name="finger_length" value="0.0" scope="global"/>
-    <xacro:include filename="$(find ddpg)/urdf/electric_gripper/fingers/${l_finger}.xacro" />
-    <xacro:finger_xacro gripper_side="${gripper_side}" finger_side="l" reflect="1" finger_tip="${l_finger_tip}" finger_grasp="${l_finger_grasp}" gazebo="${gazebo}"/>
-    <xacro:property name="tip_finger_length" value="${finger_length}" scope="local"/>
-
-    <!-- Right finger link -->
-    <xacro:include filename="$(find ddpg)/urdf/electric_gripper/fingers/${r_finger}.xacro" />
-    <xacro:finger_xacro gripper_side="${gripper_side}" finger_side="r" reflect="-1" finger_tip="${r_finger_tip}" finger_grasp="${r_finger_grasp}" gazebo="${gazebo}"/>
-    <xacro:if value="${finger_length > tip_finger_length}">
-        <xacro:property name="tip_finger_length" value="${finger_length}" scope="local"/>
-    </xacro:if>
-
-    <!-- Gripper Base Joint -->
-    <xacro:property name="end_of_arm_offset" value="-0.005" scope="local"/>
-    <joint name="${side}_gripper_base_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 ${end_of_arm_offset}"/>
-      <parent link="${side}_hand"/>
-      <child link="${side}_gripper_base"/>
-    </joint>
-
-    <!-- Gripper Tip joint -->
-    <xacro:property name="electric_gripper_base_length" value="0.04552" scope="local"/>
-    <joint name="${side}_gripper_tip_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 ${(-1*end_of_arm_offset)+connector_plate_length+electric_gripper_base_length+tip_finger_length}"/>
-      <parent link="${side}_gripper_base"/>
-      <child link="${side}_gripper_tip"/>
-    </joint>
-
-    <!-- Electric Gripper Tip link -->
-    <link name="${side}_gripper_tip">
-      <inertial>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <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>
-
-    <!-- Left finger -->
-    <xacro:property name="l_finger_offset" value="${((l_finger_slot-1)*0.0095) -0.011}" scope="local"/>
-    <joint name="${gripper_side}_gripper_l_finger_joint" type="prismatic">
-      <origin rpy="0 0 0" xyz="0.003 ${l_finger_offset} ${electric_gripper_base_length}"/>
-      <axis xyz="0 1 0"/>
-      <parent link="${side}_electric_gripper_base"/>
-      <child link="${gripper_side}_gripper_l_finger"/>
-      <limit effort="20.0" lower="0.0" upper="0.020833" velocity="5.0"/>
-      <dynamics damping="100.0" friction="500.0"/>
-    </joint>
-
-    <!-- Right finger -->
-    <xacro:property name="r_finger_offset" value="${((r_finger_slot-1)*-0.0095) +0.011}" scope="local"/>
-    <joint name="${gripper_side}_gripper_r_finger_joint" type="prismatic">
-      <origin rpy="0 0 0" xyz="0.003 ${r_finger_offset} ${electric_gripper_base_length}"/>
-      <axis xyz="0 1 0"/>
-      <parent link="${side}_electric_gripper_base"/>
-      <child link="${gripper_side}_gripper_r_finger"/>
-      <limit effort="20.0" lower="-0.020833" upper="0.0" velocity="5.0"/>
-      <mimic joint="${gripper_side}_gripper_l_finger_joint" multiplier="-1.0"/>
-      <dynamics damping="100.0" friction="500.0"/>
-    </joint>
-
-    <xacro:if value="${gazebo}">
-    <transmission name="gripper_${side}1">
-      <type>transmission_interface/SimpleTransmission</type>
-      <joint name="${gripper_side}_gripper_r_finger_joint">
-        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-      </joint>
-      <actuator name="gripper_${gripper_side}1_motor1">
-        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-        <mechanicalReduction>1</mechanicalReduction>
-      </actuator>
-    </transmission>
-    <transmission name="gripper_${side}2">
-      <type>transmission_interface/SimpleTransmission</type>
-      <joint name="${gripper_side}_gripper_l_finger_joint">
-        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-      </joint>
-      <actuator name="gripper_${gripper_side}1_motor2">
-        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-        <mechanicalReduction>1</mechanicalReduction>
-      </actuator>
-    </transmission>
-    <gazebo reference="${gripper_side}_gripper_r_finger_joint">
-      <implicitSpringDamper>1</implicitSpringDamper>
-    </gazebo>
-    <gazebo reference="${gripper_side}_gripper_l_finger_joint">
-      <implicitSpringDamper>1</implicitSpringDamper>
-    </gazebo>
-    </xacro:if>
-
-  </xacro:macro>
-</robot>
diff --git a/urdf/sawyer.urdf b/urdf/sawyer.urdf
deleted file mode 100644
index bb85000..0000000
--- a/urdf/sawyer.urdf
+++ /dev/null
@@ -1,530 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from ./urdf.xacro                   | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="sawyer" xmlns:xacro="http://www.ros.org/wiki/xacro">
-  <material name="black">
-    <color rgba="0 0 0 1"/>
-  </material>
-  <material name="darkgray">
-    <color rgba=".2 .2 .2 1"/>
-  </material>
-  <material name="darkred">
-    <color rgba=".5 .1 .1 1"/>
-  </material>
-  <material name="sawyer_red">
-    <color rgba=".5 .1 .1 1"/>
-  </material>
-  <material name="sawyer_gray">
-    <color rgba="0.75294 0.75294 0.75294 1"/>
-  </material>
-  <link name="base"/>
-  <link name="torso">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0 0 0"/>
-      </geometry>
-      <material name="darkgray">
-        <color rgba=".2 .2 .2 1"/>
-      </material>
-    </visual>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <link name="pedestal">
-    <visual>
-      <origin rpy="1.57079632679 0 -1.57079632679" xyz="0.26 0.345 -0.91488"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/pedestal.DAE"/>
-      </geometry>
-      <material name="darkgray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 -0.30"/>
-      <geometry>
-        <cylinder length="0.62" radius="0.18"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="60.86397744"/>
-      <inertia ixx="5.0635929" ixy="0.00103417" ixz="0.80199628" iyy="6.08689388" iyz="0.00105311" izz="4.96191932"/>
-    </inertial>
-  </link>
-  <link name="controller_box">
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.325 0 -0.38"/>
-      <geometry>
-        <box size="0.22 0.4 0.53"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="controller_box_fixed" type="fixed">
-    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-    <parent link="base"/>
-    <child link="controller_box"/>
-  </joint>
-  <link name="pedestal_feet">
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.1225 0 -0.758"/>
-      <geometry>
-        <box size="0.77 0.7 0.31"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="pedestal_feet_fixed" type="fixed">
-    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-    <parent link="base"/>
-    <child link="pedestal_feet"/>
-  </joint>
-  <joint name="torso_t0" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="base"/>
-    <child link="torso"/>
-  </joint>
-  <joint name="pedestal_fixed" type="fixed">
-    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-    <parent link="base"/>
-    <child link="pedestal"/>
-  </joint>
-  <joint name="right_arm_mount" type="fixed">
-    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-    <parent link="base"/>
-    <child link="right_arm_base_link"/>
-  </joint>
-  <link name="right_arm_base_link">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.0006241 -2.8025E-05 0.065404"/>
-      <mass value="2.0687"/>
-      <inertia ixx="0.0067599" ixy="-4.2024E-05" ixz="-6.1904E-07" iyy="0.0067877" iyz="1.5888E-05" izz="0.0074031"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/base.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.12"/>
-      <geometry>
-        <cylinder length="0.24" radius="0.08"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="right_l0">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.024366 0.010969 0.14363"/>
-      <mass value="5.3213"/>
-      <inertia ixx="0.053314" ixy="0.0047093" ixz="0.011734" iyy="0.057902" iyz="0.0080179" izz="0.023659"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_mp3/l0.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.081 0 0.237"/>
-      <geometry>
-        <sphere radius="0.07"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j0" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0.08"/>
-    <parent link="right_arm_base_link"/>
-    <child link="right_l0"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="80.0" lower="-3.0503" upper="3.0503" velocity="1.74"/>
-  </joint>
-  <link name="head">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0053207 -2.6549E-05 0.1021"/>
-      <mass value="1.5795"/>
-      <inertia ixx="0.011833" ixy="-4.4669E-06" ixz="4.9425E-05" iyy="0.0082709" iyz="4.2124E-07" izz="0.0049661"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/head.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.08"/>
-      <geometry>
-        <sphere radius="0.18"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="head_pan" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0.2965"/>
-    <parent link="right_l0"/>
-    <child link="head"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="8" lower="-5.1477" upper="0.9559" velocity="1.8"/>
-  </joint>
-  <link name="right_torso_itb">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <joint name="right_torso_itb" type="fixed">
-    <origin rpy="0 -1.57079632679 0" xyz="-.055 0 .22"/>
-    <parent link="right_l0"/>
-    <child link="right_torso_itb"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="right_l1">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.0030849 -0.026811 0.092521"/>
-      <mass value="4.505"/>
-      <inertia ixx="0.022398" ixy="-0.00023986" ixz="-0.00029362" iyy="0.014613" iyz="-0.0060875" izz="0.017295"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_mp3/l1.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.1225"/>
-      <geometry>
-        <sphere radius="0.07"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j1" type="revolute">
-    <origin rpy="-1.57079632679 1.57079632679 0" xyz="0.081 0.05 0.237"/>
-    <parent link="right_l0"/>
-    <child link="right_l1"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="80.0" lower="-3.8183" upper="2.2824" velocity="1.328"/>
-  </joint>
-  <link name="right_l2">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.00016232 -0.015346 0.13445"/>
-      <mass value="1.7251"/>
-      <inertia ixx="0.025176" ixy="4.3031E-06" ixz="1.4564E-05" iyy="0.024982" iyz="-0.0033928" izz="0.0033798"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/l2.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.14"/>
-      <geometry>
-        <cylinder length="0.35" radius="0.06"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j2" type="revolute">
-    <origin rpy="1.57079632679 0 0" xyz="0 -0.14 0.1425"/>
-    <parent link="right_l1"/>
-    <child link="right_l2"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="40.0" lower="-3.0514" upper="3.0514" velocity="1.957"/>
-  </joint>
-  <link name="right_l3">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.0046678 -0.028394 -0.083127"/>
-      <mass value="2.4743"/>
-      <inertia ixx="0.0099549" ixy="-1.7675E-05" ixz="0.00027521" iyy="0.0064248" iyz="0.0030853" izz="0.0067746"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/l3.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 -0.01 -0.12"/>
-      <geometry>
-        <sphere radius="0.06"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j3" type="revolute">
-    <origin rpy="-1.57079632679 0 0" xyz="0 -0.042 0.26"/>
-    <parent link="right_l2"/>
-    <child link="right_l3"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="40.0" lower="-3.0514" upper="3.0514" velocity="1.957"/>
-  </joint>
-  <link name="right_l4">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.0027794 0.0076558 0.13273"/>
-      <mass value="1.0433"/>
-      <inertia ixx="0.012913" ixy="2.573E-05" ixz="0.00017705" iyy="0.012939" iyz="0.0011152" izz="0.0012405"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/l4.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.14"/>
-      <geometry>
-        <cylinder length="0.36" radius="0.045"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j4" type="revolute">
-    <origin rpy="1.57079632679 0 0" xyz="0 -0.125 -0.1265"/>
-    <parent link="right_l3"/>
-    <child link="right_l4"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="9.0" lower="-2.9842" upper="2.9842" velocity="3.485"/>
-  </joint>
-  <link name="right_arm_itb">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <joint name="right_arm_itb" type="fixed">
-    <origin rpy="0 -1.57079632679 0" xyz="-.055 0 .075"/>
-    <parent link="right_l4"/>
-    <child link="right_arm_itb"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="right_l5">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0062067 -0.024291 0.075564"/>
-      <mass value="1.5343"/>
-      <inertia ixx="0.0046072" ixy="0.00012002" ixz="5.3065E-05" iyy="0.0028725" iyz="-0.0011886" izz="0.003101"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/l5.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.01 0 0.09"/>
-      <geometry>
-        <sphere radius="0.06"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j5" type="revolute">
-    <origin rpy="-1.57079632679 0 0" xyz="0 0.031 0.275"/>
-    <parent link="right_l4"/>
-    <child link="right_l5"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="9.0" lower="-2.9842" upper="2.9842" velocity="3.485"/>
-  </joint>
-  <link name="right_hand_camera"/>
-  <joint name="right_hand_camera" type="fixed">
-    <origin rpy="0 1.57079632679 0" xyz="0.039552 -0.033 0.0695"/>
-    <parent link="right_l5"/>
-    <child link="right_hand_camera"/>
-    <axis xyz="0 0 0"/>
-  </joint>
-  <link name="right_wrist"/>
-  <joint name="right_wrist" type="fixed">
-    <origin rpy="1.57079632679 0 0" xyz="0 0 0.10541"/>
-    <parent link="right_l5"/>
-    <child link="right_wrist"/>
-    <axis xyz="0 0 0"/>
-  </joint>
-  <link name="right_l6">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-8.0726E-06 0.0085838 -0.0049566"/>
-      <mass value="0.3292"/>
-      <inertia ixx="0.00031105" ixy="1.4771E-06" ixz="-3.7074E-07" iyy="0.00021549" iyz="-8.4533E-06" izz="0.00035976"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_mp1/l6.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 -0.005"/>
-      <geometry>
-        <cylinder length="0.05" radius="0.065"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j6" type="revolute">
-    <origin rpy="-1.57079632679 -0.17453 3.1416" xyz="0 -0.11 0.1053"/>
-    <parent link="right_l5"/>
-    <child link="right_l6"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="9.0" lower="-4.7104" upper="4.7104" velocity="4.545"/>
-  </joint>
-  <link name="right_hand">
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.02"/>
-      <geometry>
-        <cylinder length="0.04" radius="0.06"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="1.0E-08 1.0E-08 1.0E-08"/>
-      <mass value="1.0E-08"/>
-      <inertia ixx="1.0E-08" ixy="1.0E-08" ixz="1.0E-08" iyy="1.0E-08" iyz="1.0E-08" izz="1.0E-08"/>
-    </inertial>
-  </link>
-  <joint name="right_hand" type="fixed">
-    <origin rpy="0 0 1.570796" xyz="0 0 0.0245"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_l6"/>
-    <child link="right_hand"/>
-  </joint>
-  <link name="screen">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.24 0.14 0.002"/>
-      </geometry>
-      <material name="darkgray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <sphere radius="0.001"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-  </link>
-  <joint name="display_joint" type="fixed">
-    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.03 0.0 0.105"/>
-    <parent link="head"/>
-    <child link="screen"/>
-  </joint>
-  <link name="head_camera"/>
-  <joint name="head_camera" type="fixed">
-    <origin rpy="-2.1293 0 -1.57079632679" xyz="0.0228027 0 0.216572"/>
-    <parent link="head"/>
-    <child link="head_camera"/>
-    <axis xyz="0 0 0"/>
-  </joint>
-
-<!-- added -->
-    <!-- <link name="controller_box"> -->
-    <!--   <collision> -->
-    <!--     <origin rpy="0 0 0" xyz="-0.325 0 -0.38"/> -->
-    <!--     <geometry> -->
-    <!--       <box size="0.22 0.4 0.53"/> -->
-    <!--     </geometry> -->
-    <!--   </collision> -->
-    <!-- </link> -->
-    <!-- <joint name="controller_box_fixed" type="fixed"> -->
-    <!--   <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/> -->
-    <!--   <parent link="base"/> -->
-    <!--   <child link="controller_box"/> -->
-    <!-- </joint> -->
-    <!-- <link name="pedestal_feet"> -->
-    <!--   <collision> -->
-    <!--     <origin rpy="0 0 0" xyz="-0.1225 0 -0.758"/> -->
-    <!--     <geometry> -->
-    <!--       <box size="0.77 0.7 0.31"/> -->
-    <!--     </geometry> -->
-    <!--   </collision> -->
-    <!-- </link> -->
-    <!-- <joint name="pedestal_feet_fixed" type="fixed"> -->
-    <!--   <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/> -->
-    <!--   <parent link="base"/> -->
-    <!--   <child link="pedestal_feet"/> -->
-    <!-- </joint> -->
-
-    <joint name="right_gripper_base" type="fixed">
-      <child link="right_gripper_base">
-      </child>
-      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.00500000000000000">
-      </origin>
-      <parent link="right_hand">
-      </parent>
-    </joint>
-    <joint name="right_gripper_endpoint" type="fixed">
-      <child link="right_gripper">
-      </child>
-      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0500000000000000">
-      <!-- <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.2000000000000000"> -->
-      </origin>
-      <parent link="right_gripper_base">
-      </parent>
-    </joint>
-    <link name="right_gripper_base">
-      <collision>
-        <geometry>
-          <cylinder length="0.118" radius="0.070">
-          </cylinder>
-        </geometry>
-        <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.00500000000000000">
-        </origin>
-      </collision>
-      <inertial>
-        <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0">
-        </inertia>
-        <mass value="0.440000000000000">
-        </mass>
-        <origin rpy="0.0 0.0 0.0" xyz="0.00280000000000000 -0.000400000000000000 0.0322000000000000">
-        </origin>
-      </inertial>
-      <visual>
-        <geometry>
-          <mesh filename="package://intera_tools_description/meshes/electric_gripper/electric_gripper_w_fingers.DAE" scale="1.0 1.0 1.0">
-          </mesh>
-        </geometry>
-        <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.00100000000000000">
-        </origin>
-      </visual>
-    </link>
-    <link name="right_gripper">
-      <collision>
-        <geometry>
-          <box size="0.0100000000000000 0.0100000000000000 0.0100000000000000">
-          </box>
-        </geometry>
-        <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0">
-        </origin>
-      </collision>
-      <inertial>
-        <inertia ixx="1.00000000000000e-08" ixy="0.0" ixz="0.0" iyy="1.00000000000000e-08" iyz="0.0" izz="1.00000000000000e-08">
-        </inertia>
-        <mass value="0.0">
-        </mass>
-        <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0">
-        </origin>
-      </inertial>
-      <visual>
-        <geometry>
-          <box size="0.0150000000000000 0.0150000000000000 0.0150000000000000">
-          </box>
-        </geometry>
-        <material name="darkgrey">
-          <color rgba="0.200000000000000 0.300000000000000 0.300000000000000 1.0">
-          </color>
-        </material>
-        <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0">
-        </origin>
-      </visual>
-    </link>
-
-</robot>
diff --git a/urdf/sawyer_base.gazebo.xacro b/urdf/sawyer_base.gazebo.xacro
deleted file mode 100644
index 101f102..0000000
--- a/urdf/sawyer_base.gazebo.xacro
+++ /dev/null
@@ -1,258 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="sawyer" xmlns:xacro="http://www.ros.org/wiki/xacro">
-  <xacro:arg name="pedestal" default="true"/>
-  <xacro:arg name="static" default="true"/>
-  <!-- ros_control plugin -->
-  <gazebo>
-    <plugin name="sawyer_ros_control" filename="libsawyer_gazebo_ros_control.so">
-      <robotNamespace>/robot</robotNamespace>
-      <robotSimType>sawyer_gazebo/SawyerRobotHWSim</robotSimType>
-      <controlPeriod>0.001</controlPeriod>
-      <eStopTopic>sim_estop</eStopTopic>
-    </plugin>
-  </gazebo>
-  <!-- Gazebo-Specific Link Properties -->
-<xacro:if value="$(arg static)">
-  <link name="world"/>
-  <joint name="fixed" type="fixed">
-      <parent link="world"/>
-      <child link="base"/>
-  </joint>
-</xacro:if>
-<xacro:if value="$(arg pedestal)">
-  <gazebo reference="pedestal">
-    <material>Gazebo/FlatBlack</material>
-  </gazebo>
-</xacro:if>
-
-  <gazebo reference="torso">
-    <self_collide>true</self_collide>
-  </gazebo>
-  <gazebo reference="head_pan">
-    <implicitSpringDamper>1</implicitSpringDamper>
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <gazebo reference="right_j0">
-    <implicitSpringDamper>1</implicitSpringDamper>
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <gazebo reference="right_j1">
-    <implicitSpringDamper>1</implicitSpringDamper>
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <gazebo reference="right_j2">
-    <implicitSpringDamper>1</implicitSpringDamper>
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <gazebo reference="right_j3">
-    <implicitSpringDamper>1</implicitSpringDamper>
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <gazebo reference="right_j4">
-    <implicitSpringDamper>1</implicitSpringDamper>
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <gazebo reference="right_j5">
-    <implicitSpringDamper>1</implicitSpringDamper>
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-  <gazebo reference="right_j6">
-    <implicitSpringDamper>1</implicitSpringDamper>
-    <provideFeedback>true</provideFeedback>
-  </gazebo>
-   <joint name="head_camera_optical_joint" type="fixed">
-    <origin xyz="0 0 0" rpy="${-pi} ${-pi/2.0} ${-pi/2.0}"/>
-    <parent link="head_camera"/>
-    <child link="head_camera_optical"/>
-  </joint>
-  <link name="head_camera_optical"/>
-  <gazebo reference="head_camera_optical">
-    <sensor name="head_camera" type="camera">
-      <update_rate>10</update_rate>
-      <camera name="head_camera">
-        <horizontal_fov>2.00787026</horizontal_fov>
-        <image>
-          <width>1280</width>
-          <height>800</height>
-          <format>R8G8B8</format>
-        </image>
-        <clip>
-          <near>0.1</near>
-          <far>50</far>
-        </clip>
-        <!-- Uncomment to add image Distortion -->
-        <!-- distortion>
-          <k1>0.6545419633392568</k1>
-          <k2>0.012904541485280972</k2>
-          <k3>-0.012279942338550547</k3>
-          <p1>-0.00036475699823703154</p1>
-          <p2>3.334461262764956e-05</p2>
-          <center>677.8808657338167 382.02964533321426</center>
-        </distortion -->
-      </camera>
-      <!-- Uncomment to visualize image inside Gazebo -->
-      <!-- visualize>true</visualize -->
-      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>10</updateRate>
-        <cameraName>head_camera</cameraName>
-        <imageTopicName>/io/internal_camera/head_camera/image_raw</imageTopicName>
-        <cameraInfoTopicName>/io/internal_camera/head_camera/camera_info</cameraInfoTopicName>
-        <frameName>head_camera</frameName>
-        <focalLength>407.391526</focalLength>
-        <!-- Uncomment to add image Distortion params to camera_info topic -->
-        <!-- distortionK1>0.6545419633392568</distortionK1>
-        <distortionK2>0.012904541485280972</distortionK2>
-        <distortionK3>-0.012279942338550547</distortionK3>
-        <distortionT1>-0.00036475699823703154</distortionT1>
-        <distortionT2>3.334461262764956e-05</distortionT2>
-        <Cx>677.8808657338167</Cx>
-        <Cy>382.02964533321426</Cy -->
-      </plugin>
-    </sensor>
-  </gazebo>
-  <joint name="right_hand_camera_optical_joint" type="fixed">
-    <origin xyz="0 0 0" rpy="${-pi} ${-pi/2.0} ${-pi/2.0}"/>
-    <parent link="right_hand_camera"/>
-    <child link="right_hand_camera_optical"/>
-  </joint>
-  <link name="right_hand_camera_optical"/>
-  <gazebo reference="right_hand_camera_optical">
-    <sensor name="right_hand_camera" type="camera">
-      <update_rate>10.0</update_rate>
-      <camera name="right_hand_camera">
-        <horizontal_fov>1.0992892</horizontal_fov>
-        <image>
-          <width>752</width>
-          <height>480</height>
-          <format>L8</format>
-        </image>
-        <clip>
-          <near>0.02</near>
-          <far>10</far>
-        </clip>
-        <!-- Uncomment to add image Distortion -->
-        <!-- distortion>
-          <k1>-0.442</k1>
-          <k2>0.273</k2>
-          <k3>-0.123</k3>
-          <p1>0.0</p1>
-          <p2>0.0</p2>
-          <center>365.734985 237.988998</center>
-        </distortion -->
-      </camera>
-      <!-- Uncomment to visualize image inside Gazebo -->
-      <!-- visualize>true</visualize -->
-      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>10.0</updateRate>
-        <cameraName>right_hand_camera</cameraName>
-        <imageTopicName>/io/internal_camera/right_hand_camera/image_raw</imageTopicName>
-        <cameraInfoTopicName>/io/internal_camera/right_hand_camera/camera_info</cameraInfoTopicName>
-        <frameName>right_hand_camera</frameName>
-        <focalLength>613.760435</focalLength>
-        <!-- Uncomment to add image Distortion params to camera_info topic -->
-        <!-- distortionK1>-0.442</distortionK1>
-        <distortionK2>0.273</distortionK2>
-        <distortionK3>-0.123</distortionK3>
-        <distortionT1>0.0</distortionT1>
-        <distortionT2>0.0</distortionT2>
-        <Cx>365.734985</Cx>
-        <Cy>237.988998</Cy -->
-      </plugin>
-    </sensor>
-  </gazebo>
-  <gazebo reference="screen">
-    <visual>
-      <plugin name="screen_video_controller" filename="libgazebo_ros_video.so">
-        <height>600</height>
-        <width>1024</width>
-        <topicName>/robot/head_display</topicName>
-        <robotNamespace>/robot</robotNamespace>
-      </plugin>
-    </visual>
-  </gazebo>
-  <!-- Transmissions -->
-  <!-- Right Arm -->
-  <transmission name="right_tran0">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_j0">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor0">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="right_tran1">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_j1">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor1">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="right_tran2">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_j2">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor2">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="right_tran3">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_j3">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor3">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="right_tran4">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_j4">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor4">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="right_tran5">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_j5">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor5">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="right_tran6">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="right_j6">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="right_motor6">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <!-- Head -->
-  <transmission name="head0">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="head_pan">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="head_motor0">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-</robot>
diff --git a/urdf/sawyer_base.urdf.xacro b/urdf/sawyer_base.urdf.xacro
deleted file mode 100644
index 1539fb1..0000000
--- a/urdf/sawyer_base.urdf.xacro
+++ /dev/null
@@ -1,534 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="sawyer" xmlns:xacro="http://www.ros.org/wiki/xacro">
-  <xacro:arg name="gazebo" default="false"/>
-  <xacro:arg name="pedestal" default="true"/>
-  <material name="black">
-    <color rgba="0 0 0 1"/>
-  </material>
-  <material name="darkgray">
-    <color rgba=".2 .2 .2 1"/>
-  </material>
-  <material name="darkred">
-    <color rgba=".5 .1 .1 1"/>
-  </material>
-  <material name="sawyer_red">
-    <color rgba=".5 .1 .1 1"/>
-  </material>
-  <material name="sawyer_gray">
-    <color rgba="0.75294 0.75294 0.75294 1"/>
-  </material>
-  <link name="base"/>
-  <link name="torso">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="darkgray">
-        <color rgba=".2 .2 .2 1"/>
-      </material>
-    </visual>
-<xacro:unless value="$(arg gazebo)">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-</xacro:unless>
-  </link>
-<xacro:if value="$(arg pedestal)">
-  <link name="pedestal">
-    <visual>
-      <origin rpy="1.57079632679 0 -1.57079632679" xyz="0.26 0.345 -0.91488"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/pedestal.DAE"/>
-      </geometry>
-      <material name="darkgray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.02 0 -0.29"/>
-      <geometry>
-        <cylinder length="0.62" radius="0.18"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 -0.5"/>
-      <mass value="60.86397744"/>
-      <inertia ixx="5.0635929" ixy="0.00103417" ixz="0.80199628" iyy="6.08689388" iyz="0.00105311" izz="4.96191932"/>
-    </inertial>
-  </link>
-  <link name="controller_box">
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.325 0 -0.38"/>
-      <geometry>
-        <box size="0.22 0.4 0.53"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="controller_box_fixed" type="fixed">
-    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-    <parent link="base"/>
-    <child link="controller_box"/>
-  </joint>
-  <link name="pedestal_feet">
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.1225 0 -0.758"/>
-      <geometry>
-        <box size="0.77 0.7 0.31"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="pedestal_feet_fixed" type="fixed">
-    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-    <parent link="base"/>
-    <child link="pedestal_feet"/>
-  </joint>
-  <joint name="pedestal_fixed" type="fixed">
-    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-    <parent link="base"/>
-    <child link="pedestal"/>
-  </joint>
-</xacro:if>
-  <joint name="right_arm_mount" type="fixed">
-    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-    <parent link="base"/>
-    <child link="right_arm_base_link"/>
-  </joint>
-  <joint name="torso_t0" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="base"/>
-    <child link="torso"/>
-  </joint>
-  <link name="right_arm_base_link">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.0006241 -2.8025E-05 0.065404"/>
-      <mass value="2.0687"/>
-      <inertia ixx="0.0067599" ixy="-4.2024E-05" ixz="-6.1904E-07" iyy="0.0067877" iyz="1.5888E-05" izz="0.0074031"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/base.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.12"/>
-      <geometry>
-        <cylinder length="0.24" radius="0.08"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="right_l0">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.024366 0.010969 0.14363"/>
-      <mass value="5.3213"/>
-      <inertia ixx="0.053314" ixy="0.0047093" ixz="0.011734" iyy="0.057902" iyz="0.0080179" izz="0.023659"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_mp3/l0.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.08 0 0.23"/>
-      <geometry>
-        <sphere radius="0.07"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j0" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0.08"/>
-    <parent link="right_arm_base_link"/>
-    <child link="right_l0"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="80.0" lower="-3.0503" upper="3.0503" velocity="1.74"/>
-<xacro:if value="$(arg gazebo)">
-    <dynamics damping="10.0" friction="5.0"/>
-</xacro:if>
-  </joint>
-  <link name="head">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0053207 -2.6549E-05 0.1021"/>
-      <mass value="1.5795"/>
-      <inertia ixx="0.011833" ixy="-4.4669E-06" ixz="4.9425E-05" iyy="0.0082709" iyz="4.2124E-07" izz="0.0049661"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/head.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.08"/>
-      <geometry>
-        <sphere radius="0.18"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="head_pan" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0.2965"/>
-    <parent link="right_l0"/>
-    <child link="head"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="8" lower="-5.0952" upper="0.9064" velocity="1.8"/>
-<xacro:if value="$(arg gazebo)">
-    <dynamics damping="1.0" friction="0.5"/>
-</xacro:if>
-  </joint>
-  <link name="right_torso_itb">
-<xacro:unless value="$(arg gazebo)">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-</xacro:unless>
-  </link>
-  <joint name="right_torso_itb" type="fixed">
-    <origin rpy="0 -1.57079632679 0" xyz="-.055 0 .22"/>
-    <parent link="right_l0"/>
-    <child link="right_torso_itb"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="right_l1">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.0030849 -0.026811 0.092521"/>
-      <mass value="4.505"/>
-      <inertia ixx="0.022398" ixy="-0.00023986" ixz="-0.00029362" iyy="0.014613" iyz="-0.0060875" izz="0.017295"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_mp3/l1.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.1225"/>
-      <geometry>
-        <sphere radius="0.07"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j1" type="revolute">
-    <origin rpy="-1.57079632679 1.57079632679 0" xyz="0.081 0.05 0.237"/>
-    <parent link="right_l0"/>
-    <child link="right_l1"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="80.0" lower="-3.8095" upper="2.2736" velocity="1.328"/>
-<xacro:if value="$(arg gazebo)">
-    <dynamics damping="5.0" friction="2.0"/>
-</xacro:if>
-  </joint>
-  <link name="right_l2">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.00016044 -0.014967 0.13582"/>
-      <mass value="1.745"/>
-      <inertia ixx="0.025506" ixy="4.4101E-06" ixz="1.4955E-05" iyy="0.0253" iyz="-0.0033204" izz="0.0034179"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/l2.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.08"/>
-      <geometry>
-        <cylinder length="0.34" radius="0.06"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j2" type="revolute">
-    <origin rpy="1.57079632679 0 0" xyz="0 -0.14 0.1425"/>
-    <parent link="right_l1"/>
-    <child link="right_l2"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="40.0" lower="-3.0426" upper="3.0426" velocity="1.957"/>
-<xacro:if value="$(arg gazebo)">
-    <dynamics damping="5.0" friction="2.0"/>
-</xacro:if>
-  </joint>
-  <link name="right_l3">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.0048135 -0.0281 -0.084154"/>
-      <mass value="2.5097"/>
-      <inertia ixx="0.01016" ixy="-9.7452E-06" ixz="0.00026624" iyy="0.0065685" iyz="0.0030316" izz="0.0069078"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/l3.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 -0.01 -0.12"/>
-      <geometry>
-        <sphere radius="0.06"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j3" type="revolute">
-    <origin rpy="-1.57079632679 0 0" xyz="0 -0.042 0.26"/>
-    <parent link="right_l2"/>
-    <child link="right_l3"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="40.0" lower="-3.0439" upper="3.0439" velocity="1.957"/>
- <xacro:if value="$(arg gazebo)">
-    <dynamics damping="1.0" friction="0.5"/>
-</xacro:if>
-  </joint>
-  <link name="right_l4">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.0018844 0.0069001 0.1341"/>
-      <mass value="1.1136"/>
-      <inertia ixx="0.013557" ixy="1.8109E-05" ixz="0.00013523" iyy="0.013555" iyz="0.0010561" izz="0.0013658"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/l4.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.11"/>
-      <geometry>
-        <cylinder length="0.30" radius="0.045"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j4" type="revolute">
-    <origin rpy="1.57079632679 0 0" xyz="0 -0.125 -0.1265"/>
-    <parent link="right_l3"/>
-    <child link="right_l4"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="9.0" lower="-2.9761" upper="2.9761" velocity="3.485"/>
-<xacro:if value="$(arg gazebo)">
-    <dynamics damping="2.0" friction="1.0"/>
-</xacro:if>
-  </joint>
-  <link name="right_arm_itb">
-<xacro:unless value="$(arg gazebo)">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-</xacro:unless>
-  </link>
-  <joint name="right_arm_itb" type="fixed">
-    <origin rpy="0 -1.57079632679 0" xyz="-.055 0 .075"/>
-    <parent link="right_l4"/>
-    <child link="right_arm_itb"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="right_l5">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0061133 -0.023697 0.076416"/>
-      <mass value="1.5625"/>
-      <inertia ixx="0.0047328" ixy="0.00011526" ixz="4.6269E-05" iyy="0.0029676" iyz="-0.0011557" izz="0.0031762"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_pv/l5.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.1"/>
-      <geometry>
-        <sphere radius="0.06"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j5" type="revolute">
-    <origin rpy="-1.57079632679 0 0" xyz="0 0.031 0.275"/>
-    <parent link="right_l4"/>
-    <child link="right_l5"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="9.0" lower="-2.9761" upper="2.9761" velocity="3.485"/>
-<xacro:if value="$(arg gazebo)">
-    <dynamics damping="1.0" friction="0.5"/>
-</xacro:if>
-  </joint>
-  <link name="right_hand_camera"/>
-  <joint name="right_hand_camera" type="fixed">
-    <origin rpy="0 1.57079632679 0" xyz="0.039552 -0.033 0.0695"/>
-    <parent link="right_l5"/>
-    <child link="right_hand_camera"/>
-    <axis xyz="0 0 0"/>
-  </joint>
-  <link name="right_wrist"/>
-  <joint name="right_wrist" type="fixed">
-    <origin rpy="1.57079632679 0 0" xyz="0 0 0.10541"/>
-    <parent link="right_l5"/>
-    <child link="right_wrist"/>
-    <axis xyz="0 0 0"/>
-  </joint>
-  <link name="right_l6">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-8.0726E-06 0.0085838 -0.0049566"/>
-      <mass value="0.3292"/>
-      <inertia ixx="0.00031105" ixy="1.4771E-06" ixz="-3.7074E-07" iyy="0.00021549" iyz="-8.4533E-06" izz="0.00035976"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://sawyer_description/meshes/sawyer_mp1/l6.DAE"/>
-      </geometry>
-      <material name="sawyer_red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0.015 -0.01"/>
-      <geometry>
-        <cylinder length="0.05" radius="0.055"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j6" type="revolute">
-    <origin rpy="-1.57079632679 -0.17453 3.1416" xyz="0 -0.11 0.1053"/>
-    <parent link="right_l5"/>
-    <child link="right_l6"/>
-    <axis xyz="0 0 1"/>
-<xacro:if value="$(arg gazebo)">
-    <limit effort="9.0" lower="-3.14" upper="3.14" velocity="4.545"/>
-    <dynamics damping="1.0" friction="0.5"/>
-</xacro:if>
-<xacro:unless value="$(arg gazebo)">
-    <limit effort="9.0" lower="-4.7124" upper="4.7124" velocity="4.545"/>
-</xacro:unless>
-  </joint>
-  <link name="right_hand">
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.02"/>
-      <geometry>
-        <cylinder length="0.03" radius="0.035"/>
-      </geometry>
-    </collision>
-<xacro:if value="$(arg gazebo)">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0.1 0"/>
-      <mass value="0.01"/>
-      <inertia ixx="0.01" ixy="0.01" ixz="0.01" iyy="0.01" iyz="0.01" izz="0.01"/>
-  </inertial>
-</xacro:if>
-<xacro:unless value="$(arg gazebo)">
-    <inertial>
-      <origin rpy="0 0 0" xyz="1.0E-08 1.0E-08 1.0E-08"/>
-      <mass value="1.0E-08"/>
-      <inertia ixx="1.0E-08" ixy="1.0E-08" ixz="1.0E-08" iyy="1.0E-08" iyz="1.0E-08" izz="1.0E-08"/>
-    </inertial>
-</xacro:unless>
-  </link>
-  <joint name="right_hand" type="fixed">
-    <origin rpy="0 0 1.570796" xyz="0 0 0.0245"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_l6"/>
-    <child link="right_hand"/>
-  </joint>
-  <link name="right_l1_2">
-<xacro:unless value="$(arg gazebo)">
-    <inertial>
-      <origin rpy="0 0 0" xyz="1.0E-08 1.0E-08 1.0E-08"/>
-      <mass value="1.0E-08"/>
-      <inertia ixx="1.0E-08" ixy="1.0E-08" ixz="1.0E-08" iyy="1.0E-08" iyz="1.0E-08" izz="1.0E-08"/>
-    </inertial>
-</xacro:unless>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.035"/>
-      <geometry>
-        <cylinder length="0.14" radius="0.07"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j1_2" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="right_l1"/>
-    <child link="right_l1_2"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="right_l2_2">
-<xacro:unless value="$(arg gazebo)">
-    <inertial>
-      <origin rpy="0 0 0" xyz="1.0E-08 1.0E-08 1.0E-08"/>
-      <mass value="1.0E-08"/>
-      <inertia ixx="1.0E-08" ixy="1.0E-08" ixz="1.0E-08" iyy="1.0E-08" iyz="1.0E-08" izz="1.0E-08"/>
-    </inertial>
-</xacro:unless>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.26"/>
-      <geometry>
-        <sphere radius="0.06"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j2_2" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="right_l2"/>
-    <child link="right_l2_2"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="right_l4_2">
-<xacro:unless value="$(arg gazebo)">
-    <inertial>
-      <origin rpy="0 0 0" xyz="1.0E-08 1.0E-08 1.0E-08"/>
-      <mass value="1.0E-08"/>
-      <inertia ixx="1.0E-08" ixy="1.0E-08" ixz="1.0E-08" iyy="1.0E-08" iyz="1.0E-08" izz="1.0E-08"/>
-    </inertial>
-</xacro:unless>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0.01 0.26"/>
-      <geometry>
-        <sphere radius="0.06"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_j4_2" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="right_l4"/>
-    <child link="right_l4_2"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="screen">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.24 0.14 0.002"/>
-      </geometry>
-      <material name="darkgray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <sphere radius="0.001"/>
-      </geometry>
-    </collision>
-<xacro:unless value="$(arg gazebo)">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0001"/>
-      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
-    </inertial>
-</xacro:unless>
-  </link>
-  <joint name="display_joint" type="fixed">
-    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.03 0.0 0.105"/>
-    <parent link="head"/>
-    <child link="screen"/>
-  </joint>
-  <link name="head_camera"/>
-  <joint name="head_camera" type="fixed">
-    <origin rpy="-2.1293 0 -1.57079632679" xyz="0.0228027 0 0.216572"/>
-    <parent link="head"/>
-    <child link="head_camera"/>
-    <axis xyz="0 0 0"/>
-  </joint>
-</robot>
diff --git a/urdf/sawyer_electric_gripper.urdf.xacro b/urdf/sawyer_electric_gripper.urdf.xacro
deleted file mode 100644
index c763fbc..0000000
--- a/urdf/sawyer_electric_gripper.urdf.xacro
+++ /dev/null
@@ -1,23 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="end_effector" xmlns:xacro="http://www.ros.org/wiki/xacro">
-  <xacro:arg name="gazebo" default="true"/>
-  <xacro:if value="$(arg gazebo)">
-    <xacro:property name="gazebo_tag" value="true" scope="local"/>
-  </xacro:if>
-  <xacro:unless value="$(arg gazebo)">
-    <xacro:property name="gazebo_tag" value="true" scope="local"/>
-  </xacro:unless>
-
-  <xacro:include filename="$(find ddpg)/urdf/electric_gripper/rethink_electric_gripper.xacro" />
-  <xacro:rethink_electric_gripper side="right"
-                                  gazebo="${gazebo_tag}"
-                                  use_connector_plate="true"
-                                  l_finger="standard_narrow"
-                                  l_finger_slot="2"
-                                  l_finger_tip="basic_soft_tip"
-                                  l_finger_grasp="inner"
-                                  r_finger="standard_narrow"
-                                  r_finger_slot="2"
-                                  r_finger_tip="basic_soft_tip"
-                                  r_finger_grasp="inner"/>
-</robot>