diff --git a/config/hand.xacro b/config/hand.xacro
new file mode 100644
index 0000000..c72e93e
--- /dev/null
+++ b/config/hand.xacro
@@ -0,0 +1,47 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+    <xacro:macro name="hand">
+        <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+        <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+        <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+        <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+        <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+        <group name="hand">
+            <link name="panda_hand"/>
+            <link name="panda_leftfinger"/>
+            <link name="panda_rightfinger"/>
+        </group>
+        <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+        <group_state name="open" group="hand">
+            <joint name="panda_finger_joint1" value="0.04"/>
+            <joint name="panda_finger_joint2" value="0.04"/>
+        </group_state>
+        <group_state name="close" group="hand">
+            <joint name="panda_finger_joint1" value="0"/>
+            <joint name="panda_finger_joint2" value="0"/>
+        </group_state>
+        <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+        <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
+        <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
+        <passive_joint name="panda_finger_joint2"/>
+        <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+        <disable_collisions link1="panda_hand" link2="panda_leftfinger" reason="Adjacent"/>
+        <disable_collisions link1="panda_hand" link2="panda_link4" reason="Never"/>
+        <disable_collisions link1="panda_hand" link2="panda_link5" reason="Default"/>
+        <disable_collisions link1="panda_hand" link2="panda_link6" reason="Never"/>
+        <disable_collisions link1="panda_hand" link2="panda_link7" reason="Default"/>
+        <disable_collisions link1="panda_hand" link2="panda_link8" reason="Adjacent"/>
+        <disable_collisions link1="panda_hand" link2="panda_rightfinger" reason="Adjacent"/>
+        <disable_collisions link1="panda_leftfinger" link2="panda_link4" reason="Never"/>
+        <disable_collisions link1="panda_leftfinger" link2="panda_link5" reason="Default"/>
+        <disable_collisions link1="panda_leftfinger" link2="panda_link6" reason="Never"/>
+        <disable_collisions link1="panda_leftfinger" link2="panda_link7" reason="Never"/>
+        <disable_collisions link1="panda_leftfinger" link2="panda_link8" reason="Never"/>
+        <disable_collisions link1="panda_leftfinger" link2="panda_rightfinger" reason="Default"/>
+        <disable_collisions link1="panda_link4" link2="panda_rightfinger" reason="Never"/>
+        <disable_collisions link1="panda_link5" link2="panda_rightfinger" reason="Default"/>
+        <disable_collisions link1="panda_link6" link2="panda_rightfinger" reason="Never"/>
+        <disable_collisions link1="panda_link7" link2="panda_rightfinger" reason="Never"/>
+        <disable_collisions link1="panda_link8" link2="panda_rightfinger" reason="Never"/>
+    </xacro:macro>
+</robot>
diff --git a/config/panda.srdf b/config/panda.srdf
deleted file mode 100644
index c9d7ea0..0000000
--- a/config/panda.srdf
+++ /dev/null
@@ -1,97 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<!--This does not replace URDF, and is not an extension of URDF.
-    This is a format for representing semantic information about the robot structure.
-    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
--->
-<robot name="panda">
-    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
-    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
-    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
-    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
-    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
-    <group name="panda_arm">
-        <joint name="virtual_joint"/>
-        <joint name="panda_joint1"/>
-        <joint name="panda_joint2"/>
-        <joint name="panda_joint3"/>
-        <joint name="panda_joint4"/>
-        <joint name="panda_joint5"/>
-        <joint name="panda_joint6"/>
-        <joint name="panda_joint7"/>
-        <joint name="panda_joint8"/>
-    </group>
-    <group name="hand">
-        <link name="panda_hand"/>
-        <link name="panda_leftfinger"/>
-        <link name="panda_rightfinger"/>
-    </group>
-    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-    <group_state name="ready" group="panda_arm">
-        <joint name="panda_joint1" value="0"/>
-        <joint name="panda_joint2" value="-0.785"/>
-        <joint name="panda_joint3" value="0"/>
-        <joint name="panda_joint4" value="-2.356"/>
-        <joint name="panda_joint5" value="0"/>
-        <joint name="panda_joint6" value="1.571"/>
-        <joint name="panda_joint7" value="-0.7855"/>
-    </group_state>
-    <group_state name="extended" group="panda_arm">
-        <joint name="panda_joint1" value="0"/>
-        <joint name="panda_joint2" value="-0.3927"/>
-        <joint name="panda_joint3" value="0"/>
-        <joint name="panda_joint4" value="-0.7855"/>
-        <joint name="panda_joint5" value="0"/>
-        <joint name="panda_joint6" value="3.142"/>
-        <joint name="panda_joint7" value="-0.785"/>
-    </group_state>
-    <group_state name="open" group="hand">
-        <joint name="panda_finger_joint1" value="0.04"/>
-    </group_state>
-    <group_state name="close" group="hand">
-        <joint name="panda_finger_joint1" value="0"/>
-    </group_state>
-    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
-    <end_effector name="hand" parent_link="panda_link8" group="hand"/>
-    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="panda_link0"/>
-    <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
-    <passive_joint name="panda_finger_joint2"/>
-    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-    <disable_collisions link1="panda_hand" link2="panda_leftfinger" reason="Adjacent"/>
-    <disable_collisions link1="panda_hand" link2="panda_link4" reason="Never"/>
-    <disable_collisions link1="panda_hand" link2="panda_link5" reason="Default"/>
-    <disable_collisions link1="panda_hand" link2="panda_link6" reason="Never"/>
-    <disable_collisions link1="panda_hand" link2="panda_link7" reason="Default"/>
-    <disable_collisions link1="panda_hand" link2="panda_link8" reason="Adjacent"/>
-    <disable_collisions link1="panda_hand" link2="panda_rightfinger" reason="Adjacent"/>
-    <disable_collisions link1="panda_leftfinger" link2="panda_link4" reason="Never"/>
-    <disable_collisions link1="panda_leftfinger" link2="panda_link5" reason="Default"/>
-    <disable_collisions link1="panda_leftfinger" link2="panda_link6" reason="Never"/>
-    <disable_collisions link1="panda_leftfinger" link2="panda_link7" reason="Never"/>
-    <disable_collisions link1="panda_leftfinger" link2="panda_link8" reason="Never"/>
-    <disable_collisions link1="panda_leftfinger" link2="panda_rightfinger" reason="Default"/>
-    <disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent"/>
-    <disable_collisions link1="panda_link0" link2="panda_link2" reason="Never"/>
-    <disable_collisions link1="panda_link0" link2="panda_link3" reason="Never"/>
-    <disable_collisions link1="panda_link0" link2="panda_link4" reason="Never"/>
-    <disable_collisions link1="panda_link1" link2="panda_link2" reason="Adjacent"/>
-    <disable_collisions link1="panda_link1" link2="panda_link3" reason="Default"/>
-    <disable_collisions link1="panda_link1" link2="panda_link4" reason="Never"/>
-    <disable_collisions link1="panda_link2" link2="panda_link3" reason="Adjacent"/>
-    <disable_collisions link1="panda_link2" link2="panda_link4" reason="Never"/>
-    <disable_collisions link1="panda_link3" link2="panda_link4" reason="Adjacent"/>
-    <disable_collisions link1="panda_link3" link2="panda_link6" reason="Never"/>
-    <disable_collisions link1="panda_link4" link2="panda_link5" reason="Adjacent"/>
-    <disable_collisions link1="panda_link4" link2="panda_link6" reason="Never"/>
-    <disable_collisions link1="panda_link4" link2="panda_link7" reason="Never"/>
-    <disable_collisions link1="panda_link4" link2="panda_link8" reason="Never"/>
-    <disable_collisions link1="panda_link4" link2="panda_rightfinger" reason="Never"/>
-    <disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent"/>
-    <disable_collisions link1="panda_link5" link2="panda_link7" reason="Default"/>
-    <disable_collisions link1="panda_link5" link2="panda_rightfinger" reason="Default"/>
-    <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent"/>
-    <disable_collisions link1="panda_link6" link2="panda_rightfinger" reason="Never"/>
-    <disable_collisions link1="panda_link7" link2="panda_link8" reason="Adjacent"/>
-    <disable_collisions link1="panda_link7" link2="panda_rightfinger" reason="Never"/>
-    <disable_collisions link1="panda_link8" link2="panda_rightfinger" reason="Never"/>
-</robot>
diff --git a/config/panda.srdf.xacro b/config/panda.srdf.xacro
new file mode 100644
index 0000000..0541bf3
--- /dev/null
+++ b/config/panda.srdf.xacro
@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+    <xacro:include filename="$(find panda_moveit_config)/config/panda_arm.xacro" />
+    <xacro:panda_arm />
+
+    <xacro:arg name="hand" default="false" />
+
+    <!--Add the hand if people request it-->
+    <xacro:if value="$(arg hand)">
+        <xacro:include filename="$(find panda_moveit_config)/config/hand.xacro" />
+        <xacro:hand />
+    </xacro:if>
+
+    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <xacro:if value="$(arg hand)">
+        <end_effector name="hand" parent_link="panda_link8" group="hand"/>
+    </xacro:if>
+
+</robot>
diff --git a/config/panda_arm.xacro b/config/panda_arm.xacro
new file mode 100644
index 0000000..34dfef8
--- /dev/null
+++ b/config/panda_arm.xacro
@@ -0,0 +1,70 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+    <xacro:macro name="panda_arm">
+        <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+        <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+        <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+        <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+        <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+        <group name="panda_arm">
+            <joint name="virtual_joint"/>
+            <joint name="panda_joint1"/>
+            <joint name="panda_joint2"/>
+            <joint name="panda_joint3"/>
+            <joint name="panda_joint4"/>
+            <joint name="panda_joint5"/>
+            <joint name="panda_joint6"/>
+            <joint name="panda_joint7"/>
+            <joint name="panda_joint8"/>
+        </group>
+        <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+        <group_state name="ready" group="panda_arm">
+            <joint name="panda_joint1" value="0"/>
+            <joint name="panda_joint2" value="-0.785"/>
+            <joint name="panda_joint3" value="0"/>
+            <joint name="panda_joint4" value="-2.356"/>
+            <joint name="panda_joint5" value="0"/>
+            <joint name="panda_joint6" value="1.571"/>
+            <joint name="panda_joint7" value="-0.7855"/>
+        </group_state>
+        <group_state name="extended" group="panda_arm">
+            <joint name="panda_joint1" value="0"/>
+            <joint name="panda_joint2" value="-0.3927"/>
+            <joint name="panda_joint3" value="0"/>
+            <joint name="panda_joint4" value="-0.7855"/>
+            <joint name="panda_joint5" value="0"/>
+            <joint name="panda_joint6" value="3.142"/>
+            <joint name="panda_joint7" value="-0.785"/>
+        </group_state>
+        <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
+        <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="panda_link0"/>
+        <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+        <disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent"/>
+        <disable_collisions link1="panda_link0" link2="panda_link2" reason="Never"/>
+        <disable_collisions link1="panda_link0" link2="panda_link3" reason="Never"/>
+        <disable_collisions link1="panda_link0" link2="panda_link4" reason="Never"/>
+        <disable_collisions link1="panda_link1" link2="panda_link2" reason="Adjacent"/>
+        <disable_collisions link1="panda_link1" link2="panda_link3" reason="Default"/>
+        <disable_collisions link1="panda_link1" link2="panda_link4" reason="Never"/>
+        <disable_collisions link1="panda_link2" link2="panda_link3" reason="Adjacent"/>
+        <disable_collisions link1="panda_link2" link2="panda_link4" reason="Never"/>
+        <disable_collisions link1="panda_link3" link2="panda_link4" reason="Adjacent"/>
+        <disable_collisions link1="panda_link3" link2="panda_link6" reason="Never"/>
+        <disable_collisions link1="panda_link4" link2="panda_link5" reason="Adjacent"/>
+        <disable_collisions link1="panda_link4" link2="panda_link6" reason="Never"/>
+        <disable_collisions link1="panda_link4" link2="panda_link7" reason="Never"/>
+        <disable_collisions link1="panda_link4" link2="panda_link8" reason="Never"/>
+        <disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent"/>
+        <disable_collisions link1="panda_link5" link2="panda_link7" reason="Default"/>
+        <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent"/>
+        <disable_collisions link1="panda_link7" link2="panda_link8" reason="Adjacent"/>
+        <!-- Fix self-collisions
+            NOTE: The code below fixes self-collisions that are introduced because the
+            collision geometries provided by Franka are too course (see
+            https://github.com/ros-planning/panda_moveit_config/issues/72#issuecomment-768472329
+            for more information).
+        -->
+        <disable_collisions link1="panda_link5" link2="panda_link8" reason="Default" />
+        <disable_collisions link1="panda_link6" link2="panda_link8" reason="Default"/>
+    </xacro:macro>
+</robot>
diff --git a/config/panda_gripper_moveit_controllers.yaml b/config/panda_gripper_moveit_controllers.yaml
new file mode 100644
index 0000000..8a6efb6
--- /dev/null
+++ b/config/panda_gripper_moveit_controllers.yaml
@@ -0,0 +1,20 @@
+controller_list:
+  - name: position_joint_trajectory_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - panda_joint1
+      - panda_joint2
+      - panda_joint3
+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7
+  - name: franka_gripper
+    action_ns: gripper_action
+    type: GripperCommand
+    default: true
+    joints:
+      - panda_finger_joint1
+      - panda_finger_joint2
diff --git a/config/panda_moveit_controllers.yaml b/config/panda_moveit_controllers.yaml
new file mode 100644
index 0000000..3cd123f
--- /dev/null
+++ b/config/panda_moveit_controllers.yaml
@@ -0,0 +1,13 @@
+controller_list:
+  - name: position_joint_trajectory_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - panda_joint1
+      - panda_joint2
+      - panda_joint3
+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7
diff --git a/config/ros_controllers.yaml b/config/ros_controllers.yaml
index a67b290..6ad2d06 100644
--- a/config/ros_controllers.yaml
+++ b/config/ros_controllers.yaml
@@ -17,57 +17,4 @@ hardware_interface:
     - panda_joint6
     - panda_joint7
     - panda_finger_joint1
-  sim_control_mode: 1  # 0: position, 1: velocity
-# Publish all joint states
-# Creates the /joint_states topic necessary in ROS
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-controller_list:
-  []
-position_joint_trajectory_controller:
-  type: position_controllers/JointTrajectoryController
-  joints:
-    - panda_joint1
-    - panda_joint2
-    - panda_joint3
-    - panda_joint4
-    - panda_joint5
-    - panda_joint6
-    - panda_joint7
-  gains:
-    panda_joint1:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    panda_joint2:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    panda_joint3:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    panda_joint4:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    panda_joint5:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    panda_joint6:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    panda_joint7:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
\ No newline at end of file
+  sim_control_mode: 1  # 0: position, 1: velocity
\ No newline at end of file
diff --git a/config/sensors_3d.yaml b/config/sensors_3d.yaml
deleted file mode 100644
index 0713b61..0000000
--- a/config/sensors_3d.yaml
+++ /dev/null
@@ -1,23 +0,0 @@
-# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
-sensors:
-  - filtered_cloud_topic: filtered_cloud
-    max_range: 5.0
-    max_update_rate: 1.0
-    padding_offset: 0.1
-    padding_scale: 1.0
-    point_cloud_topic: /head_mount_kinect/depth_registered/points
-    point_subsample: 1
-    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
-  - far_clipping_plane_distance: 5.0
-    filtered_cloud_topic: filtered_cloud
-    image_topic: /head_mount_kinect/depth_registered/image_raw
-    max_range: 5.0
-    max_update_rate: 1.0
-    near_clipping_plane_distance: 0.3
-    padding_offset: 0.03
-    padding_scale: 4.0
-    point_cloud_topic: /head_mount_kinect/depth_registered/points
-    point_subsample: 1
-    queue_size: 5
-    sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
-    shadow_threshold: 0.2
\ No newline at end of file
diff --git a/config/sensors_kinect_depthmap.yaml b/config/sensors_kinect_depthmap.yaml
new file mode 100644
index 0000000..9538fe0
--- /dev/null
+++ b/config/sensors_kinect_depthmap.yaml
@@ -0,0 +1,11 @@
+sensors:
+  - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+    image_topic: /camera/depth_registered/image_raw
+    queue_size: 5
+    near_clipping_plane_distance: 0.3
+    far_clipping_plane_distance: 5.0
+    shadow_threshold: 0.2
+    padding_scale: 4.0
+    padding_offset: 0.03
+    max_update_rate: 1.0
+    filtered_cloud_topic: filtered_cloud
diff --git a/config/sensors_kinect_pointcloud.yaml b/config/sensors_kinect_pointcloud.yaml
new file mode 100644
index 0000000..92e7095
--- /dev/null
+++ b/config/sensors_kinect_pointcloud.yaml
@@ -0,0 +1,9 @@
+sensors:
+  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+    point_cloud_topic: /camera/depth_registered/points
+    max_range: 5.0
+    point_subsample: 1
+    padding_offset: 0.1
+    padding_scale: 1.0
+    max_update_rate: 1.0
+    filtered_cloud_topic: filtered_cloud
diff --git a/launch/chomp_planning_pipeline.launch.xml b/launch/chomp_planning_pipeline.launch.xml
index bbcaba9..8482643 100644
--- a/launch/chomp_planning_pipeline.launch.xml
+++ b/launch/chomp_planning_pipeline.launch.xml
@@ -2,12 +2,12 @@
   <!-- CHOMP Plugin for MoveIt -->
   <arg name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
 
-  <arg name="start_state_max_bounds_error" value="0.1" />
-  <arg name="jiggle_fraction" value="0.05" />
+  <arg name="start_state_max_bounds_error" default="0.1" />
+  <arg name="jiggle_fraction" default="0.05" />
   <!-- The request adapters (plugins) used when planning.
        ORDER MATTERS -->
   <arg name="planning_adapters"
-       value="default_planner_request_adapters/AddTimeParameterization
+       default="default_planner_request_adapters/AddTimeParameterization
               default_planner_request_adapters/ResolveConstraintFrames
               default_planner_request_adapters/FixWorkspaceBounds
               default_planner_request_adapters/FixStartStateBounds
diff --git a/launch/demo.launch b/launch/demo.launch
index a88e16f..5b9a2c7 100644
--- a/launch/demo.launch
+++ b/launch/demo.launch
@@ -11,12 +11,17 @@
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />
 
+  <!-- By default we will load the gripper -->
+  <arg name="load_gripper" default="true" />
+
   <!-- By default, we will load or override the robot_description -->
   <arg name="load_robot_description" default="true"/>
 
   <!-- Set execution mode for fake execution controllers -->
   <arg name="execution_type" default="interpolate" />
 
+  <!-- Other arguments -->
+  <arg name="rviz_tutorial" default="false" />
   <!--
   By default, hide joint_state_publisher's GUI
 
@@ -51,11 +56,13 @@
     <arg name="info" value="true"/>
     <arg name="debug" value="$(arg debug)"/>
     <arg name="pipeline" value="$(arg pipeline)"/>
+    <arg name="load_gripper" value="$(arg load_gripper)"/>
     <arg name="load_robot_description" value="$(arg load_robot_description)"/>
   </include>
 
   <!-- Run Rviz and load the default config to see the state of the move_group node -->
   <include file="$(dirname)/moveit_rviz.launch" if="$(arg use_rviz)">
+    <arg name="rviz_tutorial" value="$(arg rviz_tutorial)"/>
     <arg name="rviz_config" value="$(find panda_moveit_config)/launch/moveit.rviz"/>
     <arg name="debug" value="$(arg debug)"/>
   </include>
diff --git a/launch/demo_gazebo.launch b/launch/demo_gazebo.launch
deleted file mode 100644
index caae7cd..0000000
--- a/launch/demo_gazebo.launch
+++ /dev/null
@@ -1,74 +0,0 @@
-<launch>
-
-  <!-- By default, we do not start a database (it can be large) -->
-  <arg name="db" default="false" />
-  <!-- Allow user to specify database location -->
-  <arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" />
-
-  <!-- By default, we are not in debug mode -->
-  <arg name="debug" default="false" />
-
-  <!-- By default, we won't load or override the robot_description -->
-  <arg name="load_robot_description" default="false"/>
-
-  <!--
-  By default, hide joint_state_publisher's GUI
-
-  MoveIt's "demo" mode replaces the real robot driver with the joint_state_publisher.
-  The latter one maintains and publishes the current joint configuration of the simulated robot.
-  It also provides a GUI to move the simulated robot around "manually".
-  This corresponds to moving around the real robot without the use of MoveIt.
-  -->
-  <arg name="use_gui" default="false" />
-
-  <!-- Gazebo specific options -->
-  <arg name="gazebo_gui" default="true"/>
-  <arg name="paused" default="false"/>
-  <!-- By default, use the urdf location provided from the package -->
-  <arg name="urdf_path" default="$(find franka_description)/robots/panda_arm.urdf.xacro"/>
-
-  <!-- launch the gazebo simulator and spawn the robot -->
-  <include file="$(dirname)/gazebo.launch" >
-    <arg name="paused" value="$(arg paused)"/>
-    <arg name="gazebo_gui" value="$(arg gazebo_gui)"/>
-    <arg name="urdf_path" value="$(arg urdf_path)"/>
-  </include>
-
-  <!-- If needed, broadcast static tf for robot root -->
-    <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world panda_link0" />
-
-
-  <!-- We do not have a robot connected, so publish fake joint states -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
-    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
-    <rosparam param="source_list">[/joint_states]</rosparam>
-  </node>
-  <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
-    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
-    <rosparam param="source_list">[/joint_states]</rosparam>
-  </node>
-
-  <!-- Given the published joint states, publish tf for the robot links -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
-
-  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
-  <include file="$(dirname)/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="false"/>
-    <arg name="info" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
-    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
-  </include>
-
-  <!-- Run Rviz and load the default config to see the state of the move_group node -->
-  <include file="$(dirname)/moveit_rviz.launch">
-    <arg name="rviz_config" value="$(find panda_moveit_config)/launch/moveit.rviz"/>
-    <arg name="debug" value="$(arg debug)"/>
-  </include>
-
-  <!-- If database loading was enabled, start mongodb as well -->
-  <include file="$(dirname)/default_warehouse_db.launch" if="$(arg db)">
-    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
-  </include>
-
-</launch>
diff --git a/launch/gazebo.launch b/launch/gazebo.launch
deleted file mode 100644
index 16adbad..0000000
--- a/launch/gazebo.launch
+++ /dev/null
@@ -1,23 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-  <arg name="paused" default="false"/>
-  <arg name="gazebo_gui" default="true"/>
-  <arg name="urdf_path" default="$(find franka_description)/robots/panda_arm.urdf.xacro"/>
-
-  <!-- startup simulated world -->
-  <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <arg name="world_name" default="worlds/empty.world"/>
-    <arg name="paused" value="$(arg paused)"/>
-    <arg name="gui" value="$(arg gazebo_gui)"/>
-  </include>
-
-  <!-- send robot urdf to param server -->
-  <param name="robot_description" textfile="$(arg urdf_path)" />
-
-  <!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
-  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 0"
-    respawn="false" output="screen" />
-
-  <include file="$(dirname)/ros_controllers.launch"/>
-
-</launch>
diff --git a/launch/move_group.launch b/launch/move_group.launch
index 154ce53..f746839 100644
--- a/launch/move_group.launch
+++ b/launch/move_group.launch
@@ -21,6 +21,10 @@
 
   <arg name="capabilities" default=""/>
   <arg name="disable_capabilities" default=""/>
+
+  <!--Other settings-->
+  <arg name="load_gripper" default="true" />
+
   <!-- load these non-default MoveGroup capabilities (space seperated) -->
   <!--
   <arg name="capabilities" value="
@@ -41,6 +45,7 @@
   <!-- load URDF, SRDF and joint_limits configuration -->
   <include file="$(dirname)/planning_context.launch">
     <arg name="load_robot_description" value="$(arg load_robot_description)" />
+    <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
 
   <!-- Planning Pipelines -->
@@ -62,8 +67,8 @@
     </include>
 
     <!-- Support custom planning pipeline -->
-    <include if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"
-	     file="$(dirname)/planning_pipeline.launch.xml">
+    <include ns="$(arg pipeline)" if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"
+       file="$(dirname)/planning_pipeline.launch.xml">
       <arg name="pipeline" value="$(arg pipeline)" />
     </include>
   </group>
@@ -71,7 +76,8 @@
   <!-- Trajectory Execution Functionality -->
   <include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
     <arg name="moveit_manage_controllers" value="true" />
-    <arg name="moveit_controller_manager" value="panda" unless="$(arg fake_execution)"/>
+    <arg name="moveit_controller_manager" value="panda" if="$(eval not arg('fake_execution') and not arg('load_gripper'))"/>
+    <arg name="moveit_controller_manager" value="panda_gripper" if="$(eval not arg('fake_execution') and arg('load_gripper'))"/>
     <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
     <arg name="execution_type" value="$(arg execution_type)" />
   </include>
diff --git a/launch/moveit.rviz b/launch/moveit.rviz
index c4d6ae5..3f11f09 100644
--- a/launch/moveit.rviz
+++ b/launch/moveit.rviz
@@ -3,10 +3,9 @@ Panels:
     Help Height: 84
     Name: Displays
     Property Tree Widget:
-      Expanded:
-        - /MotionPlanning1
-      Splitter Ratio: 0.74256
-    Tree Height: 664
+      Expanded: ~
+      Splitter Ratio: 0.7425600290298462
+    Tree Height: 425
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -14,6 +13,12 @@ Panels:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
+  - Class: rviz_visual_tools/RvizVisualToolsGui
+    Name: RvizVisualToolsGui
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
@@ -23,7 +28,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.03
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -35,655 +40,233 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Class: moveit_rviz_plugin/MotionPlanning
+    - Class: rviz/MarkerArray
       Enabled: true
-      MoveIt_Goal_Tolerance: 0
-      MoveIt_Planning_Time: 5
-      MoveIt_Use_Constraint_Aware_IK: true
-      MoveIt_Warehouse_Host: 127.0.0.1
-      MoveIt_Warehouse_Port: 33829
-      Name: MotionPlanning
-      Planned Path:
-        Links:
-          base_bellow_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_footprint:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          double_stereo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_ir_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_rgb_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_prosilica_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_plate_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_tilt_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          laser_tilt_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          sensor_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          torso_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        Loop Animation: true
-        Robot Alpha: 0.5
-        Show Robot Collision: false
-        Show Robot Visual: true
-        Show Trail: false
-        State Display Time: 0.05 s
-        Trajectory Topic: move_group/display_planned_path
-      Planning Metrics:
-        Payload: 1
-        Show Joint Torques: false
-        Show Manipulability: false
-        Show Manipulability Index: false
-        Show Weight Limit: false
-      Planning Request:
-        Colliding Link Color: 255; 0; 0
-        Goal State Alpha: 1
-        Goal State Color: 250; 128; 0
-        Interactive Marker Size: 0
-        Joint Violation Color: 255; 0; 255
-        Planning Group: left_arm
-        Query Goal State: true
-        Query Start State: false
-        Show Workspace: false
-        Start State Alpha: 1
-        Start State Color: 0; 255; 0
+      Marker Topic: /rviz_visual_tools
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: moveit_rviz_plugin/Trajectory
+      Color Enabled: false
+      Enabled: true
+      Interrupt Display: false
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        panda_hand:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_leftfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_rightfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Loop Animation: false
+      Name: Trajectory
+      Robot Alpha: 0.5
+      Robot Color: 150; 50; 150
+      Robot Description: robot_description
+      Show Robot Collision: false
+      Show Robot Visual: true
+      Show Trail: false
+      State Display Time: 0.05 s
+      Trail Step Size: 1
+      Trajectory Topic: move_group/display_planned_path
+      Value: true
+    - Class: moveit_rviz_plugin/PlanningScene
+      Enabled: true
+      Move Group Namespace: ""
+      Name: PlanningScene
       Planning Scene Topic: move_group/monitored_planning_scene
       Robot Description: robot_description
       Scene Geometry:
-        Scene Alpha: 1
+        Scene Alpha: 0.8999999761581421
         Scene Color: 50; 230; 50
-        Scene Display Time: 0.2
+        Scene Display Time: 0.20000000298023224
         Show Scene Geometry: true
         Voxel Coloring: Z-Axis
         Voxel Rendering: Occupied Voxels
       Scene Robot:
         Attached Body Color: 150; 50; 150
         Links:
-          base_bellow_link:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          base_footprint:
+          panda_leftfinger:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          base_link:
+          panda_link0:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          bl_caster_l_wheel_link:
+          panda_link1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          bl_caster_r_wheel_link:
+          panda_link2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          bl_caster_rotation_link:
+          panda_link3:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          br_caster_l_wheel_link:
+          panda_link4:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          br_caster_r_wheel_link:
+          panda_link5:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          br_caster_rotation_link:
+          panda_link6:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          double_stereo_link:
+          panda_link7:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          fl_caster_l_wheel_link:
+          panda_link8:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          fl_caster_r_wheel_link:
+          panda_rightfinger:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          fl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_ir_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_rgb_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_prosilica_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_plate_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_tilt_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          laser_tilt_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          sensor_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          torso_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        Robot Alpha: 0.5
-        Show Scene Robot: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
+    Default Light: true
     Fixed Frame: panda_link0
+    Frame Rate: 30
   Name: root
   Tools:
     - Class: rviz/Interact
       Hide Inactive Objects: true
     - Class: rviz/MoveCamera
     - Class: rviz/Select
+    - Class: rviz_visual_tools/KeyTool
   Value: true
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 2.9965
+      Distance: 2.827594041824341
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.113567
-        Y: 0.10592
-        Z: 2.23518e-07
+        X: 0.11356700211763382
+        Y: 0.10592000186443329
+        Z: 2.2351800055275817e-07
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.01
-      Pitch: 0.509797
-      Target Frame: /panda_link0
-      Value: XYOrbit (rviz)
-      Yaw: 5.65995
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.47020357847213745
+      Target Frame: panda_link0
+      Yaw: 6.06496000289917
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1337
+  Height: 741
   Help:
     collapsed: false
   Hide Left Dock: false
   Hide Right Dock: false
-  Motion Planning:
+  QMainWindow State: 000000ff00000000fd0000000100000000000001780000028bfc020000000efb000000100044006900730070006c006100790073010000003d0000023a000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069010000027d0000004b0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb000000120020002d00200053006c006900640065007201000002df000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016000003d80000028b00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  RvizVisualToolsGui:
+    collapsed: false
+  Trajectory - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
-  Width: 1828
-  X: 459
-  Y: -243
+  Width: 1366
+  X: 1920
+  Y: 629
diff --git a/launch/moveit_empty.rviz b/launch/moveit_empty.rviz
new file mode 100644
index 0000000..0b6a7f4
--- /dev/null
+++ b/launch/moveit_empty.rviz
@@ -0,0 +1,99 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 84
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.742560029
+    Tree Height: 1100
+  - Class: rviz/Help
+    Name: Help
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_visual_tools/RvizVisualToolsGui
+    Name: RvizVisualToolsGui
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /rviz_visual_tools
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: panda_link0
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz_visual_tools/KeyTool
+  Value: true
+  Views:
+    Current:
+      Class: rviz/XYOrbit
+      Distance: 2.82759404
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.113567002
+        Y: 0.105920002
+        Z: 2.23518001e-07
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.369797021
+      Target Frame: panda_link0
+      Value: XYOrbit (rviz)
+      Yaw: 0.246767148
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1353
+  Help:
+    collapsed: false
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000001000000000000016a000004e6fc020000000afb000000100044006900730070006c0061007900730100000042000004e6000000dc00fffffffb0000000800480065006c00700000000342000000bb0000007500fffffffb0000000a00560069006500770073000000026d000000b5000000b500fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000030d0000023d0000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006900000004bd0000008d0000004800fffffffb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb000000000000000000000463000004e600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  RvizVisualToolsGui:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1491
+  X: 1067
+  Y: 27
diff --git a/launch/moveit_rviz.launch b/launch/moveit_rviz.launch
index a4605c0..77f32ee 100644
--- a/launch/moveit_rviz.launch
+++ b/launch/moveit_rviz.launch
@@ -4,9 +4,11 @@
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
   <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
 
-  <arg name="rviz_config" default="" />
-  <arg     if="$(eval rviz_config=='')" name="command_args" value="" />
-  <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />
+  <!--Retrieve the right rviz config file-->
+  <arg name="rviz_tutorial" default="false" />
+  <arg name="rviz_config" default="$(find panda_moveit_config)/launch/moveit.rviz" />
+  <arg unless="$(arg rviz_tutorial)" name="command_args" value="-d $(arg rviz_config)"/>
+  <arg     if="$(arg rviz_tutorial)" name="command_args" value="-d $(find panda_moveit_config)/launch/moveit_empty.rviz"/>
 
   <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
         args="$(arg command_args)" output="screen">
diff --git a/launch/ompl_planning_pipeline.launch.xml b/launch/ompl_planning_pipeline.launch.xml
index 46d6f47..e9f3a4f 100644
--- a/launch/ompl_planning_pipeline.launch.xml
+++ b/launch/ompl_planning_pipeline.launch.xml
@@ -6,7 +6,7 @@
   <!-- The request adapters (plugins) used when planning with OMPL.
        ORDER MATTERS -->
   <arg name="planning_adapters"
-       value="default_planner_request_adapters/AddTimeParameterization
+       default="default_planner_request_adapters/AddTimeParameterization
               default_planner_request_adapters/ResolveConstraintFrames
               default_planner_request_adapters/FixWorkspaceBounds
               default_planner_request_adapters/FixStartStateBounds
@@ -14,8 +14,8 @@
               default_planner_request_adapters/FixStartStatePathConstraints"
               />
 
-  <arg name="start_state_max_bounds_error" value="0.1" />
-  <arg name="jiggle_fraction" value="0.05" />
+  <arg name="start_state_max_bounds_error" default="0.1" />
+  <arg name="jiggle_fraction" default="0.05" />
 
   <param name="planning_plugin" value="$(arg planning_plugin)" />
   <param name="request_adapters" value="$(arg planning_adapters)" />
diff --git a/launch/panda_control_moveit_rviz.launch b/launch/panda_control_moveit_rviz.launch
new file mode 100644
index 0000000..ce41e83
--- /dev/null
+++ b/launch/panda_control_moveit_rviz.launch
@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<!--
+  Used to control a real robot using moveit
+-->
+<launch>
+  <arg name="load_gripper" default="true" />
+  <arg name="launch_rviz" default="true" />
+  <!--
+    By default we load the franka controllers.Disable this option if the controllers are
+    running on a seperate workstation
+  -->
+  <arg name="launch_franka_control" default="true" />
+  <group if="$(arg launch_franka_control)">
+    <arg name="robot_ip" />
+  </group>
+
+  <!-- If needed, broadcast static tf for robot root -->
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world panda_link0" />
+
+  <include if="$(arg launch_franka_control)" file="$(find franka_control)/launch/franka_control.launch">
+    <arg name="robot_ip" value="$(arg robot_ip)" />
+    <arg name="load_gripper" value="$(arg load_gripper)" />
+  </include>
+
+  <include file="$(find panda_moveit_config)/launch/panda_moveit.launch">
+    <arg name="load_gripper" value="$(arg load_gripper)" />
+  </include>
+
+  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" if="$(arg launch_rviz)"/>
+</launch>
diff --git a/launch/panda_gripper_moveit_controller_manager.launch.xml b/launch/panda_gripper_moveit_controller_manager.launch.xml
new file mode 100644
index 0000000..e275653
--- /dev/null
+++ b/launch/panda_gripper_moveit_controller_manager.launch.xml
@@ -0,0 +1,12 @@
+<launch>
+
+  <!-- this argument is not used here, only necessary so that this launch file has the same args as fake_moveit_controller_manager.launch -->
+  <arg name="execution_type" default="unused" />
+
+  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
+  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
+  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
+
+  <!-- load controller_list -->
+  <rosparam file="$(find panda_moveit_config)/config/panda_gripper_moveit_controllers.yaml"/>
+</launch>
diff --git a/launch/panda_moveit.launch b/launch/panda_moveit.launch
new file mode 100644
index 0000000..3010bd7
--- /dev/null
+++ b/launch/panda_moveit.launch
@@ -0,0 +1,10 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="load_gripper" default="true" />
+
+  <include file="$(dirname)/ros_controllers.launch"/>
+
+  <include file="$(find panda_moveit_config)/launch/move_group.launch">
+    <arg name="load_gripper" value="$(arg load_gripper)" />
+  </include>
+</launch>
diff --git a/launch/panda_moveit_controller_manager.launch.xml b/launch/panda_moveit_controller_manager.launch.xml
index febf584..f1123a7 100644
--- a/launch/panda_moveit_controller_manager.launch.xml
+++ b/launch/panda_moveit_controller_manager.launch.xml
@@ -9,5 +9,5 @@
   <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
 
   <!-- loads ros_controllers to the param server -->
-  <rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml"/>
+  <rosparam file="$(find panda_moveit_config)/config/panda_moveit_controllers.yaml"/>
 </launch>
diff --git a/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
index 82a5a48..b6bf529 100644
--- a/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
+++ b/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
@@ -5,7 +5,7 @@
 
   <!-- The request adapters (plugins) used when planning.
        ORDER MATTERS -->
-  <arg name="planning_adapters" value="" />
+  <arg name="planning_adapters" default="" />
 
   <param name="planning_plugin" value="$(arg planning_plugin)" />
   <param name="request_adapters" value="$(arg planning_adapters)" />
diff --git a/launch/planning_context.launch b/launch/planning_context.launch
index ef17b2b..27d5a3a 100644
--- a/launch/planning_context.launch
+++ b/launch/planning_context.launch
@@ -1,15 +1,16 @@
 <launch>
   <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
+  <arg name="load_gripper" default="true" />
   <arg name="load_robot_description" default="false"/>
 
   <!-- The name of the parameter under which the URDF is loaded -->
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro hand:=true '$(find franka_description)/robots/panda_arm.urdf.xacro'"/>
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro hand:=$(arg load_gripper) '$(find franka_description)/robots/panda_arm.urdf.xacro'"/>
 
   <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" textfile="$(find panda_moveit_config)/config/panda.srdf" />
+  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda.srdf.xacro' hand:=$(arg load_gripper)"/>
 
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
diff --git a/launch/ros_controllers.launch b/launch/ros_controllers.launch
index 85f011b..590c9c9 100644
--- a/launch/ros_controllers.launch
+++ b/launch/ros_controllers.launch
@@ -6,6 +6,6 @@
 
   <!-- Load the controllers -->
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-    output="screen" args="position_joint_trajectory_controller "/>
+    output="screen" args="position_joint_trajectory_controller"/>
 
 </launch>
diff --git a/launch/sensor_manager.launch.xml b/launch/sensor_manager.launch.xml
index 17279dd..dd9d3ad 100644
--- a/launch/sensor_manager.launch.xml
+++ b/launch/sensor_manager.launch.xml
@@ -3,10 +3,13 @@
   <!-- This file makes it easy to include the settings for sensor managers -->
 
   <!-- Params for 3D sensors config -->
-  <rosparam command="load" file="$(find panda_moveit_config)/config/sensors_3d.yaml" />
+  <arg name="sensor_type" default="pointcloud"/>
+  <rosparam if="$(eval arg('sensor_type')  == 'point_cloud')" command="load" file="$(find panda_moveit_config)/config/sensors_kinect_pointcloud.yaml" />
+  <rosparam unless="$(eval arg('sensor_type')  == 'point_cloud')" command="load" file="$(find panda_moveit_config)/config/sensors_kinect_depthmap.yaml" />
 
   <!-- Params for the octomap monitor -->
   <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
+  <param name="octomap_frame" type="string" value="camera_rgb_optical_frame" />
   <param name="octomap_resolution" type="double" value="0.025" />
   <param name="max_range" type="double" value="5.0" />
 
diff --git a/package.xml b/package.xml
index d35ac69..e77ab87 100644
--- a/package.xml
+++ b/package.xml
@@ -29,9 +29,13 @@
   <run_depend>rviz</run_depend>
   <run_depend>tf2_ros</run_depend>
   <run_depend>xacro</run_depend>
+  <run_depend>moveit_visual_tools</run_depend>
+  <!-- The next 2 packages are required for the gazebo simulation.
+      We don't include them by default to prevent installing gazebo and all its dependencies. -->
+  <!-- <run_depend>joint_trajectory_controller</run_depend> -->
+  <!-- <run_depend>gazebo_ros_control</run_depend> -->
   <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
   <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
-    <run_depend>franka_description</run_depend>
-
+  <run_depend>franka_description</run_depend>
 
 </package>