diff --git a/launch/baxter.launch b/launch/baxter.launch
index 79146e6..9ae25f4 100644
--- a/launch/baxter.launch
+++ b/launch/baxter.launch
@@ -16,14 +16,14 @@
     <node pkg="rapid_pbd" type="program_execution_node" name="program_executor">
       <remap from="l_arm_joint_action" to="/robot/limb/left/follow_joint_trajectory" />
       <remap from="r_arm_joint_action" to="/robot/limb/right/follow_joint_trajectory" />
-      <remap from="l_gripper_action" to="/robot/end_effector/left_gripper/gripper_action" />
-      <remap from="r_gripper_action" to="/robot/end_effector/right_gripper/gripper_action" />
       <remap from="move_group" to="/move_group" />
     </node>
     <rosparam command="load" file="$(find rapid_pbd)/config/surface_segmentation.yaml" />
     <node pkg="rapid_pbd" type="surface_segmentation_node" name="surface_segmentation_node" args="$(arg point_cloud_topic)" />
   </group>
 
+  <node pkg="tf" type="static_transform_publisher" name="world_to_camera" args="0 0 1 0 0 0 base camera_link 10" />
+
   <!-- Depth cloud -->
   <node pkg="nodelet" type="nodelet" name="rapid_pbd_convert_metric" args="standalone depth_image_proc/convert_metric" if="$(arg sim)">
     <remap from="image_raw" to="$(arg depth_topic)" />
@@ -35,5 +35,4 @@
     <param unless="$(arg sim)" name="depth" value="$(arg depth_topic)_float" />
     <param name="camera_frame_id" value="$(arg camera_frame)" />
   </node>
-  <include file="$(find rapid_pbd)/launch/tf_frames.launch" />
 </launch>
diff --git a/launch/baxter_moveit.launch b/launch/baxter_moveit.launch
index 10931dd..35b8549 100644
--- a/launch/baxter_moveit.launch
+++ b/launch/baxter_moveit.launch
@@ -1,81 +1,28 @@
+<?xml version="1.0"?>
 <launch>
-
-  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
-  <arg name="load_robot_description" default="false"/>
+  <!-- By default we DO overwrite the URDF in order to get the grippers. -->
+  <arg name="load_robot_description" default="true"/>
   <!-- Left and right electric gripper params. Set to true to check for collisions for their links -->
-  <arg name="right_electric_gripper" default="false"/>
-  <arg name="left_electric_gripper" default="false"/>
+  <arg name="right_electric_gripper" default="true"/>
+  <arg name="left_electric_gripper" default="true"/>
   <!-- Set the kinematic tips for the left_arm and right_arm move_groups -->
   <arg name="left_tip_name" default="left_gripper"/>
   <arg name="right_tip_name" default="right_gripper"/>
-  <include file="$(find rapid_pbd)/launch/baxter_planning_context.launch" />
-
-  <!-- GDB Debug Option -->
-  <arg name="debug" default="false" />
-  <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix"
-	   value="gdb -x $(find baxter_moveit_config)/launch/gdb_settings.gdb --ex run --args" />
-
-  <!-- Verbose Mode Option -->
-  <arg name="info" default="$(arg debug)" />
-  <arg unless="$(arg info)" name="command_args" value="" />
-  <arg     if="$(arg info)" name="command_args" value="--debug" />
-
-  <!-- move_group settings -->
-  <arg name="allow_trajectory_execution" default="true"/>
-  <arg name="allow_active_sensing" default="false"/>
-  <arg name="fake_execution" default="false"/>
-  <arg name="max_safe_path_cost" default="1"/>
-  <arg name="jiggle_fraction" default="0.05" />
-  <arg name="publish_monitored_planning_scene" default="true"/>
-  <arg name="pipeline" default="$(optenv PLANNING_PIPELINE ompl)" />
-
-  <!-- Planning Functionality -->
-  <include ns="move_group" file="$(find baxter_moveit_config)/launch/planning_pipeline.launch">
-    <arg name="pipeline" value="$(arg pipeline)" />
-  </include>
-
-  <!-- Trajectory Execution Functionality -->
-  <include ns="move_group" file="$(find baxter_moveit_config)/launch/trajectory_execution.launch" if="$(arg allow_trajectory_execution)">
-    <arg name="moveit_manage_controllers" value="true" />
-    <arg name="moveit_controller_manager" value="baxter" unless="$(arg fake_execution)"/>
-    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
+  <include file="$(find baxter_moveit_config)/launch/planning_context.launch">
+    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+    <arg name="left_electric_gripper" value="$(arg left_electric_gripper)"/>
+    <arg name="right_electric_gripper" value="$(arg right_electric_gripper)"/>
+    <arg name="left_tip_name" value="$(arg left_tip_name)"/>
+    <arg name="right_tip_name" value="$(arg right_tip_name)"/>
   </include>
 
-  <!-- Sensors Functionality -->
-  <rosparam command="delete" param="move_group/sensors" />
-  <include ns="move_group" file="$(find baxter_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_active_sensing)">
-    <arg name="moveit_sensor_manager" value="baxter" />
+  <arg name="kinect" default="false" />
+  <arg name="xtion" default="false" />
+  <arg name="camera_link_pose" default="0.15 0.075 0.5 0.0 0.7854 0.0"/>
+  <include file="$(find baxter_moveit_config)/launch/move_group.launch">
+    <arg name="kinect" value="$(arg kinect)" />
+    <arg name="xtion" value="$(arg xtion)" />
+    <arg name="camera_link_pose" default="$(arg camera_link_pose)"/>
+    <arg name="allow_trajectory_execution" value="true"/>
   </include>
-
-  <!-- Start the actual move_group node/action server -->
-  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
-    <!-- Set the display variable, in case OpenGL code is used internally -->
-    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />
-
-    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
-    <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
-    <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
-
-    <!-- MoveGroup capabilities to load -->
-    <param name="capabilities" value="move_group/MoveGroupCartesianPathService
-				      move_group/MoveGroupExecuteService
-				      move_group/MoveGroupKinematicsService
-				      move_group/MoveGroupMoveAction
-				      move_group/MoveGroupPickPlaceAction
-				      move_group/MoveGroupPlanService
-				      move_group/MoveGroupQueryPlannersService
-				      move_group/MoveGroupStateValidationService
-				      move_group/MoveGroupGetPlanningSceneService
-				      move_group/ApplyPlanningSceneService
-				      move_group/ClearOctomapService
-				      " />
-
-    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
-    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
-  </node>
-
 </launch>
diff --git a/launch/tf_frames.launch b/launch/tf_frames.launch
index 418db46..a04c646 100644
--- a/launch/tf_frames.launch
+++ b/launch/tf_frames.launch
@@ -1,5 +1,5 @@
 <launch>
-  <include file="$(find rgbd_launch)/launch/kinect_frames.launch" />
+  <!--include file="$(find rgbd_launch)/launch/kinect_frames.launch" /-->
   <node pkg="tf" type="static_transform_publisher" name="world_to_camera" args="0 0 1 0 0 0 base camera_link 100" />
-  <node pkg="tf" type="static_transform_publisher" name="thermal_extrinsic_calibration_publisher" args="-0.00091294711 -0.040564451 -0.025354201  -0.03274371 -0.00118647 0.00181309 0.99932802 camera_rgb_optical_frame thermal_camera_frame 100" />
+  <!--node pkg="tf" type="static_transform_publisher" name="thermal_extrinsic_calibration_publisher" args="-0.00091294711 -0.040564451 -0.025354201  -0.03274371 -0.00118647 0.00181309 0.99932802 camera_rgb_optical_frame thermal_camera_frame 100" /-->
 </launch>
diff --git a/src/robot_config.cpp b/src/robot_config.cpp
index 7655c0b..c22f411 100644
--- a/src/robot_config.cpp
+++ b/src/robot_config.cpp
@@ -204,7 +204,6 @@ void BaxterRobotConfig::default_gripper_poses(
 void BaxterRobotConfig::joints_for_group(
     const std::string& actuator_group,
     std::vector<std::string>* joint_names) const {
-  joint_names->clear();
   if (actuator_group == Action::LEFT_ARM) {
     joint_names->push_back("left_e0");
     joint_names->push_back("left_e1");