diff --git a/src/factory_sim/config/config.yaml b/src/factory_sim/config/config.yaml
index 689e6d30..aeb6f784 100644
--- a/src/factory_sim/config/config.yaml
+++ b/src/factory_sim/config/config.yaml
@@ -128,6 +128,7 @@ objectives:
     # This plugin will load the core MoveIt Pro Behaviors.
     # Add additional plugin loaders as needed.
     core:
+      - "moveit_studio::behaviors::ConverterBehaviorsLoader"
       - "moveit_studio::behaviors::CoreBehaviorsLoader"
       - "moveit_studio::behaviors::MTCCoreBehaviorsLoader"
       - "moveit_studio::behaviors::ServoBehaviorsLoader"
diff --git a/src/factory_sim/description/box-welds.xml b/src/factory_sim/description/box-welds.xml
index 4c3df10c..813a0292 100644
--- a/src/factory_sim/description/box-welds.xml
+++ b/src/factory_sim/description/box-welds.xml
@@ -3,504 +3,504 @@
     <weld
       name="suction_cup_sphere-0-0-0-0"
       body1="sphere-0-0-0-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-0-1"
       body1="sphere-0-0-0-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-0-2"
       body1="sphere-0-0-0-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-1-0"
       body1="sphere-0-0-1-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-1-1"
       body1="sphere-0-0-1-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-1-2"
       body1="sphere-0-0-1-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-2-0"
       body1="sphere-0-0-2-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-2-1"
       body1="sphere-0-0-2-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-2-2"
       body1="sphere-0-0-2-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-3-0"
       body1="sphere-0-0-3-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-3-1"
       body1="sphere-0-0-3-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-3-2"
       body1="sphere-0-0-3-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-4-0"
       body1="sphere-0-0-4-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-4-1"
       body1="sphere-0-0-4-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-4-2"
       body1="sphere-0-0-4-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-5-0"
       body1="sphere-0-0-5-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-5-1"
       body1="sphere-0-0-5-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-0-5-2"
       body1="sphere-0-0-5-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-0-0"
       body1="sphere-0-1-0-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-0-1"
       body1="sphere-0-1-0-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-0-2"
       body1="sphere-0-1-0-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-1-0"
       body1="sphere-0-1-1-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-1-1"
       body1="sphere-0-1-1-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-1-2"
       body1="sphere-0-1-1-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-2-0"
       body1="sphere-0-1-2-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-2-1"
       body1="sphere-0-1-2-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-2-2"
       body1="sphere-0-1-2-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-3-0"
       body1="sphere-0-1-3-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-3-1"
       body1="sphere-0-1-3-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-3-2"
       body1="sphere-0-1-3-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-4-0"
       body1="sphere-0-1-4-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-4-1"
       body1="sphere-0-1-4-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-4-2"
       body1="sphere-0-1-4-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-5-0"
       body1="sphere-0-1-5-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-5-1"
       body1="sphere-0-1-5-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-0-1-5-2"
       body1="sphere-0-1-5-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-0-0"
       body1="sphere-1-0-0-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-0-1"
       body1="sphere-1-0-0-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-0-2"
       body1="sphere-1-0-0-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-1-0"
       body1="sphere-1-0-1-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-1-1"
       body1="sphere-1-0-1-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-1-2"
       body1="sphere-1-0-1-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-2-0"
       body1="sphere-1-0-2-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-2-1"
       body1="sphere-1-0-2-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-2-2"
       body1="sphere-1-0-2-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-3-0"
       body1="sphere-1-0-3-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-3-1"
       body1="sphere-1-0-3-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-3-2"
       body1="sphere-1-0-3-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-4-0"
       body1="sphere-1-0-4-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-4-1"
       body1="sphere-1-0-4-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-4-2"
       body1="sphere-1-0-4-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-5-0"
       body1="sphere-1-0-5-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-5-1"
       body1="sphere-1-0-5-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-0-5-2"
       body1="sphere-1-0-5-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-0-0"
       body1="sphere-1-1-0-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-0-1"
       body1="sphere-1-1-0-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-0-2"
       body1="sphere-1-1-0-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-1-0"
       body1="sphere-1-1-1-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-1-1"
       body1="sphere-1-1-1-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-1-2"
       body1="sphere-1-1-1-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-2-0"
       body1="sphere-1-1-2-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-2-1"
       body1="sphere-1-1-2-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-2-2"
       body1="sphere-1-1-2-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-3-0"
       body1="sphere-1-1-3-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-3-1"
       body1="sphere-1-1-3-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-3-2"
       body1="sphere-1-1-3-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-4-0"
       body1="sphere-1-1-4-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-4-1"
       body1="sphere-1-1-4-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-4-2"
       body1="sphere-1-1-4-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-5-0"
       body1="sphere-1-1-5-0"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-5-1"
       body1="sphere-1-1-5-1"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
     <weld
       name="suction_cup_sphere-1-1-5-2"
       body1="sphere-1-1-5-2"
-      body2="tool_tip"
+      body2="suction_tool_tip"
       active="false"
       torquescale="1"
     />
diff --git a/src/factory_sim/description/inspection_tool.urdf b/src/factory_sim/description/inspection_tool.urdf
new file mode 100644
index 00000000..2f04935f
--- /dev/null
+++ b/src/factory_sim/description/inspection_tool.urdf
@@ -0,0 +1,42 @@
+<?xml version="1.0" ?>
+<robot name="fanuc_tool_two" xmlns:xacro="http://wiki.ros.org/xacro">
+  <link name="inspection_tool_base">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.02" />
+      <geometry>
+        <box size="0.06 0.06 0.04"/>
+      </geometry>
+      <material name="darkgrey">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <collision>
+        <origin rpy="0 0 0" xyz="0 0 0.02" />
+        <geometry>
+            <box size="0.06 0.06 0.04"/>
+        </geometry>
+        <material name="darkgrey"/>
+    </collision>
+  </link>
+  <link name="inspection_cylinder">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.015" />
+      <geometry>
+        <box size="0.01 0.01 0.03"/>
+      </geometry>
+      <material name="darkgrey"/>
+    </visual>
+    <collision>
+        <origin rpy="0 0 0" xyz="0 0 0.015" />
+        <geometry>
+            <box size="0.01 0.01 0.03"/>
+        </geometry>
+        <material name="darkgrey"/>
+    </collision>
+  </link>
+  <joint name="base_joint" type="fixed">
+    <parent link="inspection_tool_base" />
+    <child link="inspection_cylinder" />
+    <origin rpy="0 0 0" xyz="0 0 0.04" />
+  </joint>
+</robot>
diff --git a/src/factory_sim/description/inspection_tool.xml b/src/factory_sim/description/inspection_tool.xml
new file mode 100644
index 00000000..7e3b1cd2
--- /dev/null
+++ b/src/factory_sim/description/inspection_tool.xml
@@ -0,0 +1,31 @@
+<mujoco model="fanuc_tool_two">
+  <worldbody>
+    <body
+      name="inspection_tool_base"
+      gravcomp="1"
+      pos="0.25 -0.08 0.3"
+      euler="3.1415 0 0"
+    >
+      <joint
+        type="free"
+        stiffness="0"
+        damping="0"
+        frictionloss=".001"
+        armature="0"
+      />
+      <site name="inspection_tool_attach_site" pos="0 0 0" euler="0 0 0" />
+
+      <geom type="cylinder" size="0.03 0.02" pos="0 0 0.02" rgba="0 0 1 1" />
+      <body name="inspection_cylinder" gravcomp="1" pos="0 0 0.04">
+        <geom
+          type="cylinder"
+          size="0.01 0.015"
+          pos="0 0 0.015"
+          margin="0.01"
+          gap="0.01"
+          rgba="0 0 1 1"
+        />
+      </body>
+    </body>
+  </worldbody>
+</mujoco>
diff --git a/src/factory_sim/description/picknik_fanuc.xacro b/src/factory_sim/description/picknik_fanuc.xacro
index 2328731f..038722ea 100644
--- a/src/factory_sim/description/picknik_fanuc.xacro
+++ b/src/factory_sim/description/picknik_fanuc.xacro
@@ -24,17 +24,29 @@
       </geometry>
     </visual>
     <visual>
-      <origin rpy="0 0 0" xyz="0.23 0.03 0.1" />
+      <origin rpy="0 0 0" xyz="0.23 0.05 0.1" />
       <geometry>
         <box size="0.12 0.02 0.02" />
       </geometry>
     </visual>
     <visual>
-      <origin rpy="0 0 0" xyz="0.23 -0.03 0.1" />
+      <origin rpy="0 0 0" xyz="0.23 0.11 0.1" />
       <geometry>
         <box size="0.12 0.02 0.02" />
       </geometry>
     </visual>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.23 -0.05 0.1" />
+      <geometry>
+        <box size="0.12 0.02 0.02" />
+      </geometry>
+    </visual>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.23 -0.11 0.1" />
+      <geometry>
+        <box size="0.12 0.02 0.02" />
+      </geometry>
+    </visual>    
   </link>
   <joint name="scene_joint" type="fixed">
     <parent link="world" />
diff --git a/src/factory_sim/description/scene.xml b/src/factory_sim/description/scene.xml
index a8d41717..4cc42ee2 100644
--- a/src/factory_sim/description/scene.xml
+++ b/src/factory_sim/description/scene.xml
@@ -73,9 +73,22 @@
 
     <body name="arm_pedestal" pos="0.0 0.0 0.15">
       <geom type="box" size="0.2 0.2 0.15" />
-      <geom type="box" size="0.06 0.01 0.01" pos="0.23 0.03 0.1" />
-      <geom type="box" size="0.06 0.01 0.01" pos="0.23 -0.03 0.1" />
-      <site name="tool_holder_site" pos="0.25 0 0.15" euler="3.1415 0 0" />
+      <!-- Holder for tool 1 -->
+      <geom type="box" size="0.06 0.01 0.01" pos="0.23 0.05 0.1" />
+      <geom type="box" size="0.06 0.01 0.01" pos="0.23 0.11 0.1" />
+      <site
+        name="suction_gripper_holder_site"
+        pos="0.25 0.08 0.15"
+        euler="3.1415 0 0"
+      />
+      <!-- Holder for tool 2 -->
+      <geom type="box" size="0.06 0.01 0.01" pos="0.23 -0.05 0.1" />
+      <geom type="box" size="0.06 0.01 0.01" pos="0.23 -0.11 0.1" />
+      <site
+        name="inspection_tool_holder_site"
+        pos="0.25 -0.08 0.15"
+        euler="3.1415 0 0"
+      />
     </body>
 
     <body name="arm_mount" pos="0.0 0.0 0.3">
@@ -366,19 +379,29 @@
     </body>
   </worldbody>
 
-  <!-- Add tool -->
-  <include file="tool.xml" />
+  <!-- Add tools -->
+  <include file="suction_tool.xml" />
+  <include file="inspection_tool.xml" />
 
   <!-- Define weld constraints between the robot flange (tool0) and the gripper -->
   <equality>
     <weld
-      name="tool_attachment"
+      name="tool_attachment_one"
       body1="tool0"
       body2="gripper_base"
       active="false"
       torquescale="1"
     />
   </equality>
+  <equality>
+    <weld
+      name="tool_attachment_two"
+      body1="tool0"
+      body2="inspection_tool_base"
+      active="false"
+      torquescale="1"
+    />
+  </equality>
 
   <!-- Add welds for suction gripping of cubes -->
   <include file="box-welds.xml" />
diff --git a/src/factory_sim/description/tool.urdf b/src/factory_sim/description/suction_tool.urdf
similarity index 93%
rename from src/factory_sim/description/tool.urdf
rename to src/factory_sim/description/suction_tool.urdf
index 055ee7aa..e94bc549 100644
--- a/src/factory_sim/description/tool.urdf
+++ b/src/factory_sim/description/suction_tool.urdf
@@ -1,5 +1,5 @@
 <?xml version="1.0" ?>
-<robot name="fanuc_tool" xmlns:xacro="http://wiki.ros.org/xacro">
+<robot name="fanuc_tool_one" xmlns:xacro="http://wiki.ros.org/xacro">
   <link name="gripper_base">
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0.02" />
diff --git a/src/factory_sim/description/tool.xml b/src/factory_sim/description/suction_tool.xml
similarity index 80%
rename from src/factory_sim/description/tool.xml
rename to src/factory_sim/description/suction_tool.xml
index 9ee014e5..04b27afe 100644
--- a/src/factory_sim/description/tool.xml
+++ b/src/factory_sim/description/suction_tool.xml
@@ -1,9 +1,9 @@
-<mujoco model="fanuc_tool">
+<mujoco model="fanuc_tool_one">
   <worldbody>
     <body
       name="gripper_base"
       gravcomp="1"
-      pos="0.25 0.0 0.3"
+      pos="0.25 0.08 0.3"
       euler="3.1415 0 0"
     >
       <joint
@@ -13,7 +13,7 @@
         frictionloss=".001"
         armature="0"
       />
-      <site name="tool_attach_site" pos="0 0 0" euler="0 0 0" />
+      <site name="suction_gripper_attach_site" pos="0 0 0" euler="0 0 0" />
 
       <geom type="cylinder" size="0.03 0.02" pos="0 0 0.02" rgba=".1 .1 .1 1" />
       <body name="suction_cylinder" gravcomp="1" pos="0 0 0.04">
@@ -25,7 +25,7 @@
           gap="0.01"
           rgba=".1 .1 .1 1"
         />
-        <body name="tool_tip" gravcomp="1" pos="0 0 0.055">
+        <body name="suction_tool_tip" gravcomp="1" pos="0 0 0.055">
           <geom
             class="collision"
             type="cylinder"
diff --git a/src/factory_sim/objectives/automated_pickup_tool_subtree.xml b/src/factory_sim/objectives/automated_pickup_tool_subtree.xml
new file mode 100644
index 00000000..d28f9bd0
--- /dev/null
+++ b/src/factory_sim/objectives/automated_pickup_tool_subtree.xml
@@ -0,0 +1,53 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Automated Pickup Tool Subtree">
+  <!-- ////////// -->
+  <BehaviorTree
+    ID="Automated Pickup Tool Subtree"
+    _description="Subtree for picking up the desired tool from the correct holder"
+  >
+    <Control ID="Sequence">
+      <Control ID="Fallback">
+        <Decorator
+          ID="Precondition"
+          else="FAILURE"
+          if="pickup_tool==tool_1_name"
+        >
+          <SubTree
+            ID="Pick Up Tool from Holder"
+            _collapsed="true"
+            tool_attach_site="{tool_1_pickup_site}"
+            tool_name="{tool_1_name}"
+          />
+        </Decorator>
+        <Decorator
+          ID="Precondition"
+          else="FAILURE"
+          if="pickup_tool==tool_2_name"
+        >
+          <SubTree
+            ID="Pick Up Tool from Holder"
+            _collapsed="true"
+            tool_attach_site="{tool_2_pickup_site}"
+            tool_name="{tool_2_name}"
+          />
+        </Decorator>
+        <Decorator ID="ForceFailure">
+          <Action ID="LogMessage" log_level="error" message="Unknown error" />
+        </Decorator>
+      </Control>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Automated Pickup Tool Subtree">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+      <inout_port name="pickup_tool" default="" />
+      <inout_port name="tool_1_name" default="" />
+      <inout_port name="tool_1_pickup_site" default="" />
+      <inout_port name="tool_2_name" default="" />
+      <inout_port name="tool_2_pickup_site" default="" />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/factory_sim/objectives/automated_return_held_tool.xml b/src/factory_sim/objectives/automated_return_held_tool.xml
new file mode 100644
index 00000000..87233d9d
--- /dev/null
+++ b/src/factory_sim/objectives/automated_return_held_tool.xml
@@ -0,0 +1,60 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Automated Return Held Tool">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Automated Return Held Tool"
+    _description="Returns the currently held tool to the correct holder"
+    _favorite="false"
+  >
+    <Control ID="IfThenElse">
+      <Control ID="Sequence">
+        <Action
+          ID="GetCurrentPlanningScene"
+          planning_scene_msg="{planning_scene}"
+        />
+        <Action ID="IsAnyObjectAttached" planning_scene="{planning_scene}" />
+      </Control>
+      <Control ID="IfThenElse">
+        <Control ID="Sequence">
+          <Action
+            ID="WhichObjectIsAttached"
+            object_name="{tool_name}"
+            planning_scene="{planning_scene}"
+            object_names="suction_gripper;inspector"
+          />
+        </Control>
+        <SubTree
+          ID="Automated Return Tool Subtree"
+          _collapsed="true"
+          return_tool="{tool_name}"
+          tool_1_name="suction_gripper"
+          tool_1_return_site="suction_gripper_holder_site"
+          ignore_collisions_between_these_objects_for_tool_1_return="link_5;link_6;suction_gripper"
+          tool_2_name="inspector"
+          tool_2_return_site="inspection_tool_holder_site"
+          ignore_collisions_between_these_objects_for_tool_2_return="link_5;link_6;inspector"
+        />
+        <Decorator ID="ForceFailure">
+          <Action
+            ID="LogMessage"
+            log_level="error"
+            message="PlanningScene in unknown state"
+          />
+        </Decorator>
+      </Control>
+      <Action
+        ID="LogMessage"
+        log_level="info"
+        message="Currently not holding a tool"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Automated Return Held Tool">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/factory_sim/objectives/automated_return_tool_subtree.xml b/src/factory_sim/objectives/automated_return_tool_subtree.xml
new file mode 100644
index 00000000..249032a7
--- /dev/null
+++ b/src/factory_sim/objectives/automated_return_tool_subtree.xml
@@ -0,0 +1,67 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Automated Return Tool Subtree">
+  <!-- ////////// -->
+  <BehaviorTree
+    ID="Automated Return Tool Subtree"
+    _description="Subtree for returning the currently held tool to the correct holder"
+  >
+    <Control ID="Sequence">
+      <Control ID="Fallback">
+        <Decorator
+          ID="Precondition"
+          else="FAILURE"
+          if="return_tool==tool_1_name"
+        >
+          <SubTree
+            ID="Place Tool in Tool Holder"
+            _collapsed="true"
+            tool_name="{tool_1_name}"
+            tool_return_site="{tool_1_return_site}"
+            ignore_collisions_between_these_objects="{ignore_collisions_between_these_objects_for_tool_1_return}"
+          />
+        </Decorator>
+        <Decorator
+          ID="Precondition"
+          else="FAILURE"
+          if="return_tool==tool_2_name"
+        >
+          <SubTree
+            ID="Place Tool in Tool Holder"
+            _collapsed="true"
+            tool_name="{tool_2_name}"
+            tool_return_site="{tool_2_return_site}"
+            ignore_collisions_between_these_objects="{ignore_collisions_between_these_objects_for_tool_2_return}"
+          />
+        </Decorator>
+        <Decorator ID="ForceFailure">
+          <Action
+            ID="LogMessage"
+            message="Attached tool is not in list of tool names"
+            log_level="error"
+          />
+        </Decorator>
+      </Control>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Automated Return Tool Subtree">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+      <inout_port name="return_tool" default="" />
+      <inout_port name="tool_1_name" default="" />
+      <inout_port name="tool_1_return_site" default="" />
+      <inout_port
+        name="ignore_collisions_between_these_objects_for_tool_1_return"
+        default=""
+      />
+      <inout_port name="tool_2_name" default="" />
+      <inout_port name="tool_2_return_site" default="" />
+      <inout_port
+        name="ignore_collisions_between_these_objects_for_tool_2_return"
+        default=""
+      />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/factory_sim/objectives/automated_tool_changing_example.xml b/src/factory_sim/objectives/automated_tool_changing_example.xml
new file mode 100644
index 00000000..e7145517
--- /dev/null
+++ b/src/factory_sim/objectives/automated_tool_changing_example.xml
@@ -0,0 +1,49 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Automated Tool Changing Example">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Automated Tool Changing Example"
+    _description="Pick up the black tool, place it back, then pick up the blue tool. If a tool is already attached, it will return it to the correct holder first."
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree ID="Automated Return Held Tool" _collapsed="false" />
+      <Control ID="Sequence">
+        <SubTree ID="Reset Planning Scene" _collapsed="true" />
+        <SubTree ID="Setup Initial Planning Scene" _collapsed="true" />
+      </Control>
+      <SubTree
+        ID="Automated Tool Changing Example Subtree"
+        _collapsed="false"
+        pickup_tool="suction_gripper"
+        tool_1_name="suction_gripper"
+        tool_1_pickup_site="suction_gripper_attach_site"
+        tool_1_return_site="suction_gripper_holder_site"
+        tool_2_name="inspector"
+        tool_2_pickup_site="inspection_tool_attach_site"
+        tool_2_return_site="inspection_tool_holder_site"
+        tool_names="suction_gripper;inspector"
+      />
+      <SubTree
+        ID="Automated Tool Changing Example Subtree"
+        _collapsed="true"
+        pickup_tool="inspector"
+        tool_1_name="suction_gripper"
+        tool_1_pickup_site="suction_gripper_attach_site"
+        tool_1_return_site="suction_gripper_holder_site"
+        tool_2_name="inspector"
+        tool_2_pickup_site="inspection_tool_attach_site"
+        tool_2_return_site="inspection_tool_holder_site"
+        tool_names="suction_gripper;inspector"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Automated Tool Changing Example">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/factory_sim/objectives/automated_tool_changing_example_subtree.xml b/src/factory_sim/objectives/automated_tool_changing_example_subtree.xml
new file mode 100644
index 00000000..d7f871fe
--- /dev/null
+++ b/src/factory_sim/objectives/automated_tool_changing_example_subtree.xml
@@ -0,0 +1,82 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root
+  BTCPP_format="4"
+  main_tree_to_execute="Automated Tool Changing Example Subtree"
+>
+  <!--//////////-->
+  <BehaviorTree
+    ID="Automated Tool Changing Example Subtree"
+    _description=""
+    _favorite="false"
+  >
+    <Control ID="IfThenElse">
+      <Control ID="Sequence">
+        <Action
+          ID="GetCurrentPlanningScene"
+          planning_scene_msg="{planning_scene}"
+        />
+        <Action ID="IsAnyObjectAttached" planning_scene="{planning_scene}" />
+      </Control>
+      <Control ID="IfThenElse">
+        <Action
+          ID="WhichObjectIsAttached"
+          object_name="{tool_name}"
+          object_names="{tool_names}"
+          planning_scene="{planning_scene}"
+        />
+        <Control ID="Sequence">
+          <SubTree
+            ID="Automated Return Tool Subtree"
+            _collapsed="true"
+            return_tool="{tool_name}"
+            tool_1_name="{tool_1_name}"
+            tool_1_return_site="{tool_1_return_site}"
+            ignore_collisions_between_these_objects_for_tool_1_return="link_5;link_6;suction_gripper"
+            tool_2_name="{tool_2_name}"
+            tool_2_return_site="{tool_2_return_site}"
+            ignore_collisions_between_these_objects_for_tool_2_return="link_5;link_6;inspector"
+          />
+          <SubTree
+            ID="Automated Pickup Tool Subtree"
+            _collapsed="true"
+            pickup_tool="{pickup_tool}"
+            tool_1_name="{tool_1_name}"
+            tool_1_pickup_site="{tool_1_pickup_site}"
+            tool_2_name="{tool_2_name}"
+            tool_2_pickup_site="{tool_2_pickup_site}"
+          />
+        </Control>
+        <Action
+          ID="LogMessage"
+          log_level="error"
+          message="Planning Scene is in an unexpected state"
+        />
+      </Control>
+      <SubTree
+        ID="Automated Pickup Tool Subtree"
+        _collapsed="true"
+        pickup_tool="{pickup_tool}"
+        tool_1_name="{tool_1_name}"
+        tool_1_pickup_site="{tool_1_pickup_site}"
+        tool_2_name="{tool_2_name}"
+        tool_2_pickup_site="{tool_2_pickup_site}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Automated Tool Changing Example Subtree">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+      <inout_port name="pickup_tool" default="" />
+      <inout_port name="tool_1_name" default="" />
+      <inout_port name="tool_1_pickup_site" default="" />
+      <inout_port name="tool_1_return_site" default="" />
+      <inout_port name="tool_2_name" default="" />
+      <inout_port name="tool_2_pickup_site" default="" />
+      <inout_port name="tool_2_return_site" default="" />
+      <inout_port name="tool_names" default="" />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/factory_sim/objectives/detach_and_remove_tool.xml b/src/factory_sim/objectives/detach_and_remove_tool.xml
new file mode 100644
index 00000000..3123ba4b
--- /dev/null
+++ b/src/factory_sim/objectives/detach_and_remove_tool.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Detach and Remove Tool">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Detach and Remove Tool"
+    _description="Detach a tool from the arm and remove it from the planning scene"
+  >
+    <Control ID="Fallback">
+      <Decorator ID="ForceFailure">
+        <Action ID="DetachTool" tool_name="{tool_name}" />
+      </Decorator>
+      <Action ID="RemoveToolFromScene" tool_name="{tool_name}" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Detach and Remove Tool">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Motion - Planning" />
+      </MetadataFields>
+      <inout_port name="tool_name" default="tool_1" />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/factory_sim/objectives/inspect_convex_bowl.xml b/src/factory_sim/objectives/inspect_convex_bowl.xml
new file mode 100644
index 00000000..8efe283b
--- /dev/null
+++ b/src/factory_sim/objectives/inspect_convex_bowl.xml
@@ -0,0 +1,98 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Inspect Convex Bowl">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Inspect Convex Bowl"
+    _description="Inspect the Convex bowl with the blue tool. If a tool is already attached, it will return it to the correct holder first."
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree ID="Automated Return Held Tool" _collapsed="true" />
+      <SubTree ID="Reset Planning Scene" _collapsed="true" />
+      <SubTree ID="Setup Initial Planning Scene" _collapsed="true" />
+      <SubTree
+        ID="Pick Up Tool from Holder"
+        _collapsed="true"
+        tool_name="inspector"
+        tool_attach_site="inspection_tool_attach_site"
+      />
+      <Control ID="Sequence" name="TopLevelSequence">
+        <Action
+          ID="RetrieveWaypoint"
+          joint_group_name="manipulator"
+          waypoint_joint_state="{target_joint_state}"
+          waypoint_name="View Convex"
+        />
+        <Action
+          ID="UnpackRobotJointStateMessage"
+          joint_state="{joint_state}"
+          multi_dof_joint_state="{multi_dof_joint_state}"
+          robot_joint_state="{target_joint_state}"
+        />
+        <Action
+          ID="GetCurrentPlanningScene"
+          planning_scene_msg="{planning_scene}"
+        />
+        <Action
+          ID="ComputeLinkPoseForwardKinematics"
+          joint_state="{joint_state}"
+          link_name="tool0"
+          link_pose="{link_pose}"
+          planning_scene="{planning_scene}"
+        />
+        <Action
+          ID="TransformPose"
+          input_pose="{link_pose}"
+          output_pose="{inspection_pose}"
+          translation_xyz="0;0;0.1"
+        />
+        <Action
+          ID="VisualizePose"
+          marker_lifetime="0.000000"
+          marker_name="pose"
+          marker_size="0.100000"
+          pose="{inspection_pose}"
+        />
+      </Control>
+      <Control ID="Sequence">
+        <Action
+          ID="InitializeMTCTask"
+          task="{mtc_task}"
+          trajectory_monitoring="false"
+          controller_names="/joint_trajectory_controller"
+        />
+        <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
+        <Action
+          ID="SetupMTCPlanToPose"
+          acceleration_scale_factor="0.5"
+          ik_frame="tool0"
+          keep_orientation="false"
+          keep_orientation_link_names="tool0"
+          keep_orientation_tolerance="0.100000"
+          link_padding="0.000000"
+          max_iterations="5000"
+          monitored_stage="current state"
+          planning_group_name="manipulator"
+          target_pose="{inspection_pose}"
+          task="{mtc_task}"
+          trajectory_sampling_rate="100"
+          velocity_scale_factor="0.5"
+        />
+        <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
+        <Action
+          ID="ExecuteMTCTask"
+          goal_duration_tolerance="-1.000000"
+          solution="{mtc_solution}"
+        />
+      </Control>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Inspect Convex Bowl">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/factory_sim/objectives/pick_up_tool_from_holder.xml b/src/factory_sim/objectives/pick_up_tool_from_holder.xml
index f9b0615a..957bdce0 100644
--- a/src/factory_sim/objectives/pick_up_tool_from_holder.xml
+++ b/src/factory_sim/objectives/pick_up_tool_from_holder.xml
@@ -6,18 +6,9 @@
     _description="Pick up the tool from the tool holder"
   >
     <Control ID="Sequence" name="TopLevelSequence">
-      <Control ID="Sequence" name="Assume tool is in tool holder">
-        <Action
-          ID="LogMessage"
-          log_level="info"
-          message="Assuming tool is in tool holder"
-        />
-        <SubTree ID="Reset Planning Scene" _collapsed="true" />
-        <SubTree ID="Setup Initial Planning Scene" _collapsed="true" />
-      </Control>
       <Action
         ID="CreateStampedPose"
-        reference_frame="tool_attach_site"
+        reference_frame="{tool_attach_site}"
         position_xyz="0;0;-0.1"
         stamped_pose="{approach_pose}"
       />
@@ -75,7 +66,8 @@
       <Action
         ID="AttachTool"
         parent_link_name="tool0"
-        tool_name="suction_gripper"
+        tool_name="{tool_name}"
+        allowed_collision_links="link_5;link_6"
       />
       <SubTree ID="Retract" _collapsed="true" distance="0.35" />
     </Control>
@@ -86,6 +78,8 @@
         <Metadata subcategory="Grasping" />
         <Metadata runnable="false" />
       </MetadataFields>
+      <inout_port name="tool_attach_site" default="" />
+      <inout_port name="tool_name" default="" />
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/factory_sim/objectives/place_tool_in_tool_holder.xml b/src/factory_sim/objectives/place_tool_in_tool_holder.xml
index 3605b198..d4fae672 100644
--- a/src/factory_sim/objectives/place_tool_in_tool_holder.xml
+++ b/src/factory_sim/objectives/place_tool_in_tool_holder.xml
@@ -8,7 +8,7 @@
     <Control ID="Sequence" name="TopLevelSequence">
       <Action
         ID="CreateStampedPose"
-        reference_frame="tool_holder_site"
+        reference_frame="{tool_return_site}"
         position_xyz="0;0;-0.15"
         stamped_pose="{approach_pose}"
       />
@@ -20,12 +20,18 @@
         trajectory_monitoring="false"
       />
       <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
-      <Action
-        ID="SetupMTCIgnoreCollisionsBetweenObjects"
-        allow_collision="true"
-        task="{mtc_task}"
-        object_names="link_5;link_6;suction_gripper"
-      />
+      <Decorator
+        ID="Precondition"
+        else="SUCCESS"
+        if="ignore_collisions_between_these_objects!=&quot;&quot;"
+      >
+        <Action
+          ID="SetupMTCIgnoreCollisionsBetweenObjects"
+          allow_collision="true"
+          task="{mtc_task}"
+          object_names="{ignore_collisions_between_these_objects}"
+        />
+      </Decorator>
       <Action
         ID="SetupMTCPlanToPose"
         acceleration_scale_factor="0.5"
@@ -70,7 +76,7 @@
         gripper_command_action_name="/tool_attach_controller/gripper_cmd"
         timeout="10"
       />
-      <Action ID="DetachTool" tool_name="suction_gripper" />
+      <Action ID="DetachTool" tool_name="{tool_name}" />
       <SubTree ID="Retract" _collapsed="true" distance="0.35" />
     </Control>
   </BehaviorTree>
@@ -80,6 +86,9 @@
         <Metadata runnable="false" />
         <Metadata subcategory="Grasping" />
       </MetadataFields>
+      <inout_port name="ignore_collisions_between_these_objects" default="" />
+      <inout_port name="tool_name" default="" />
+      <inout_port name="tool_return_site" default="" />
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/factory_sim/objectives/remove_tools_from_planningscene.xml b/src/factory_sim/objectives/remove_tools_from_planningscene.xml
new file mode 100644
index 00000000..eb4ca3b6
--- /dev/null
+++ b/src/factory_sim/objectives/remove_tools_from_planningscene.xml
@@ -0,0 +1,33 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Remove Tools from PlanningScene">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Remove Tools from PlanningScene"
+    _description="Remove all tools from the planning scene"
+  >
+    <Control ID="Fallback">
+      <Decorator ID="ForceFailure">
+        <SubTree
+          ID="Detach and Remove Tool"
+          _collapsed="true"
+          tool_name="suction_gripper"
+        />
+      </Decorator>
+      <Decorator ID="ForceSuccess">
+        <SubTree
+          ID="Detach and Remove Tool"
+          _collapsed="true"
+          tool_name="inspector"
+        />
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Remove Tools from PlanningScene">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Motion - Planning" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/factory_sim/objectives/reset_planning_scene.xml b/src/factory_sim/objectives/reset_planning_scene.xml
index 3cd8a2c4..848427b9 100644
--- a/src/factory_sim/objectives/reset_planning_scene.xml
+++ b/src/factory_sim/objectives/reset_planning_scene.xml
@@ -1,11 +1,23 @@
 <?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Reset Planning Scene">
   <!--//////////-->
-  <BehaviorTree ID="Reset Planning Scene" _description="" _hardcoded="false">
-    <Action
-      ID="ResetPlanningSceneObjects"
-      apply_planning_scene_service="/apply_planning_scene"
-    />
+  <BehaviorTree
+    ID="Reset Planning Scene"
+    _description="Resets the planning scene. Does not reset the simulation, so any held tools will still be attached."
+    _hardcoded="false"
+  >
+    <Control ID="Sequence">
+      <Action
+        ID="LogMessage"
+        log_level="warn"
+        message="There might be some warnings and errors when this Objective runs, as long as the Objective completes successfully, everything worked as intended."
+      />
+      <SubTree ID="Remove Tools from PlanningScene" _collapsed="false" />
+      <Action
+        ID="ResetPlanningSceneObjects"
+        apply_planning_scene_service="/apply_planning_scene"
+      />
+    </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Reset Planning Scene">
diff --git a/src/factory_sim/objectives/setup_initial_planning_scene.xml b/src/factory_sim/objectives/setup_initial_planning_scene.xml
index 48feb3f4..94775955 100644
--- a/src/factory_sim/objectives/setup_initial_planning_scene.xml
+++ b/src/factory_sim/objectives/setup_initial_planning_scene.xml
@@ -1,23 +1,36 @@
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Setup Initial Planning Scene">
   <!--//////////-->
   <BehaviorTree
     ID="Setup Initial Planning Scene"
-    _description="Sets the tool at its initial pose in the Planning Scene"
+    _description="Sets the tools at their initial pose in the Planning Scene"
     _favorite="true"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <Action
         ID="CreateStampedPose"
-        stamped_pose="{tool_pose}"
-        reference_frame="tool_attach_site"
+        stamped_pose="{suction_tool_pose}"
+        reference_frame="suction_gripper_holder_site"
       />
       <Action
         ID="AddToolToScene"
-        relative_pose="{tool_pose}"
+        relative_pose="{suction_tool_pose}"
         package_name="factory_sim"
-        tool_urdf_file_path="description/tool.urdf"
+        tool_urdf_file_path="description/suction_tool.urdf"
         tool_name="suction_gripper"
       />
+      <Action
+        ID="CreateStampedPose"
+        stamped_pose="{inspection_tool_pose}"
+        reference_frame="inspection_tool_holder_site"
+      />
+      <Action
+        ID="AddToolToScene"
+        relative_pose="{inspection_tool_pose}"
+        package_name="factory_sim"
+        tool_urdf_file_path="description/inspection_tool.urdf"
+        tool_name="inspector"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/factory_sim/objectives/tool_attachment_example.xml b/src/factory_sim/objectives/tool_attachment_example.xml
index 27a69cfe..a8a52abd 100644
--- a/src/factory_sim/objectives/tool_attachment_example.xml
+++ b/src/factory_sim/objectives/tool_attachment_example.xml
@@ -3,13 +3,30 @@
   <!--//////////-->
   <BehaviorTree
     ID="Tool Attachment Example"
-    _description="Pick up a vacuum gripper, pick up an object with it, drop the object and put the tool back in its holder"
+    _description="Attach the black tool. If a tool is already attached, it will return it to the correct holder first."
     _favorite="true"
   >
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Pick Up Tool from Holder" _collapsed="true" />
+      <SubTree ID="Automated Return Held Tool" _collapsed="true" />
+      <Control ID="Sequence">
+        <SubTree ID="Reset Planning Scene" _collapsed="true" />
+        <SubTree ID="Setup Initial Planning Scene" _collapsed="true" />
+      </Control>
+      <SubTree
+        ID="Pick Up Tool from Holder"
+        _collapsed="true"
+        ignore_collisions_between_these_objects=""
+        tool_name="suction_gripper"
+        tool_attach_site="suction_gripper_attach_site"
+      />
       <Action ID="WaitForDuration" delay_duration="2" />
-      <SubTree ID="Place Tool in Tool Holder" _collapsed="true" />
+      <SubTree
+        ID="Place Tool in Tool Holder"
+        _collapsed="false"
+        ignore_collisions_between_these_objects="link_5;link_6;suction_gripper"
+        tool_name="suction_gripper"
+        tool_return_site="suction_gripper_holder_site"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/factory_sim/waypoints/waypoints.yaml b/src/factory_sim/waypoints/waypoints.yaml
index 2b23fd38..861818f8 100644
--- a/src/factory_sim/waypoints/waypoints.yaml
+++ b/src/factory_sim/waypoints/waypoints.yaml
@@ -1,8 +1,8 @@
 - description: ''
-  favorite: true
+  favorite: false
   joint_group_names:
-    - gripper
-    - manipulator
+  - gripper
+  - manipulator
   joint_state:
     effort: []
     header:
@@ -11,19 +11,19 @@
         nanosec: 0
         sec: 0
     name:
-      - joint_1
-      - joint_2
-      - joint_3
-      - joint_4
-      - joint_5
-      - joint_6
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
     position:
-      - -1.5509811858997467
-      - 0.5153668404756869
-      - -0.7219805067810909
-      - 0.0013302776778533103
-      - -0.006224477478198248
-      - 3.1644727674098895e-08
+    - 1.1625409639061022
+    - 0.7569899884584295
+    - 0.2324359658075662
+    - -0.07323192405080868
+    - -0.9154444425780317
+    - 0.4408607192523566
     velocity: []
   multi_dof_joint_state:
     header:
@@ -35,12 +35,12 @@
     transforms: []
     twist: []
     wrench: []
-  name: View shelf
+  name: Drop block
 - description: ''
-  favorite: true
+  favorite: false
   joint_group_names:
-    - gripper
-    - manipulator
+  - gripper
+  - manipulator
   joint_state:
     effort: []
     header:
@@ -49,19 +49,19 @@
         nanosec: 0
         sec: 0
     name:
-      - joint_1
-      - joint_2
-      - joint_3
-      - joint_4
-      - joint_5
-      - joint_6
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
     position:
-      - 1.2617964010643366e-21
-      - -0.32608961429327493
-      - -0.29514299181540965
-      - 2.0932807727584565e-18
-      - -0.3220305370770435
-      - 2.083771603252834e-18
+    - 1.580174692118959
+    - -0.3174498466822826
+    - -0.5574400907076587
+    - 0.0030420865446560085
+    - -0.5759784636128362
+    - -0.0008934379999721851
     velocity: []
   multi_dof_joint_state:
     header:
@@ -73,12 +73,12 @@
     transforms: []
     twist: []
     wrench: []
-  name: View bowls
+  name: Home
 - description: ''
   favorite: false
   joint_group_names:
-    - gripper
-    - manipulator
+  - gripper
+  - manipulator
   joint_state:
     effort: []
     header:
@@ -87,19 +87,19 @@
         nanosec: 0
         sec: 0
     name:
-      - joint_1
-      - joint_2
-      - joint_3
-      - joint_4
-      - joint_5
-      - joint_6
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
     position:
-      - 1.580174692118959
-      - -0.3174498466822826
-      - -0.5574400907076587
-      - 0.0030420865446560085
-      - -0.5759784636128362
-      - -0.0008934379999721851
+    - 2.0238548295251704
+    - 1.012548784310725
+    - 0.16732230853201366
+    - 0.48572896407227517
+    - -0.3925271000306122
+    - -0.8617334001254201
     velocity: []
   multi_dof_joint_state:
     header:
@@ -111,12 +111,12 @@
     transforms: []
     twist: []
     wrench: []
-  name: Home
+  name: Left bin
 - description: ''
   favorite: false
   joint_group_names:
-    - gripper
-    - manipulator
+  - gripper
+  - manipulator
   joint_state:
     effort: []
     header:
@@ -125,19 +125,19 @@
         nanosec: 0
         sec: 0
     name:
-      - joint_1
-      - joint_2
-      - joint_3
-      - joint_4
-      - joint_5
-      - joint_6
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
     position:
-      - -4.531861149816098e-05
-      - -0.12266538508312397
-      - -0.701862360395226
-      - -3.0254405894626604e-05
-      - -0.00378749392859631
-      - -4.690750471590086e-05
+    - 0.07619213524736071
+    - -0.7105125089645793
+    - -0.39623479079949836
+    - 0.1310527185218626
+    - -0.42624024512758213
+    - -0.09171837818641333
     velocity: []
   multi_dof_joint_state:
     header:
@@ -149,12 +149,12 @@
     transforms: []
     twist: []
     wrench: []
-  name: View Convex
+  name: Registration
 - description: ''
   favorite: false
   joint_group_names:
-    - gripper
-    - manipulator
+  - gripper
+  - manipulator
   joint_state:
     effort: []
     header:
@@ -163,19 +163,19 @@
         nanosec: 0
         sec: 0
     name:
-      - joint_1
-      - joint_2
-      - joint_3
-      - joint_4
-      - joint_5
-      - joint_6
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
     position:
-      - 4.101780649531268e-37
-      - -0.4688951300482917
-      - -0.4842548345795775
-      - 6.3512581794154385e-19
-      - -0.002355461329286324
-      - 6.351037779836293e-19
+    - -0.7795842246641833
+    - -0.16101971673578258
+    - -0.4638815075836735
+    - 1.3700103295434871
+    - 0.8009294897240469
+    - -1.2239495641466815
     velocity: []
   multi_dof_joint_state:
     header:
@@ -191,8 +191,8 @@
 - description: ''
   favorite: false
   joint_group_names:
-    - gripper
-    - manipulator
+  - gripper
+  - manipulator
   joint_state:
     effort: []
     header:
@@ -201,19 +201,19 @@
         nanosec: 0
         sec: 0
     name:
-      - joint_1
-      - joint_2
-      - joint_3
-      - joint_4
-      - joint_5
-      - joint_6
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
     position:
-      - 2.0238548295251704
-      - 1.012548784310725
-      - 0.16732230853201366
-      - 0.48572896407227517
-      - -0.3925271000306122
-      - -0.8617334001254201
+    - 0.7784137024799722
+    - -0.16196107508886384
+    - -0.4693091576308623
+    - 1.7746803987816984
+    - -0.7932758541872054
+    - -1.9356390701637913
     velocity: []
   multi_dof_joint_state:
     header:
@@ -225,12 +225,12 @@
     transforms: []
     twist: []
     wrench: []
-  name: Left bin
+  name: View Convex
 - description: ''
-  favorite: false
+  favorite: true
   joint_group_names:
-    - gripper
-    - manipulator
+  - gripper
+  - manipulator
   joint_state:
     effort: []
     header:
@@ -239,19 +239,19 @@
         nanosec: 0
         sec: 0
     name:
-      - joint_1
-      - joint_2
-      - joint_3
-      - joint_4
-      - joint_5
-      - joint_6
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
     position:
-      - 0.07619213524736071
-      - -0.7105125089645793
-      - -0.39623479079949836
-      - 0.1310527185218626
-      - -0.42624024512758213
-      - -0.09171837818641333
+    - 1.5630684016798486
+    - 0.2423441041177924
+    - -0.6620668740799428
+    - -7.740231803673489e-06
+    - -0.6149111763676889
+    - 8.328343522875337e-05
     velocity: []
   multi_dof_joint_state:
     header:
@@ -263,12 +263,12 @@
     transforms: []
     twist: []
     wrench: []
-  name: Registration
+  name: View blocks
 - description: ''
   favorite: true
   joint_group_names:
-    - gripper
-    - manipulator
+  - gripper
+  - manipulator
   joint_state:
     effort: []
     header:
@@ -277,19 +277,19 @@
         nanosec: 0
         sec: 0
     name:
-      - joint_1
-      - joint_2
-      - joint_3
-      - joint_4
-      - joint_5
-      - joint_6
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
     position:
-      - 1.5630684016798486
-      - 0.2423441041177924
-      - -0.6620668740799428
-      - -7.740231803673489e-06
-      - -0.6149111763676889
-      - 8.328343522875337e-05
+    - 1.2617964010643366e-21
+    - -0.32608961429327493
+    - -0.29514299181540965
+    - 2.0932807727584565e-18
+    - -0.3220305370770435
+    - 2.083771603252834e-18
     velocity: []
   multi_dof_joint_state:
     header:
@@ -301,12 +301,12 @@
     transforms: []
     twist: []
     wrench: []
-  name: View blocks
+  name: View bowls
 - description: ''
-  favorite: false
+  favorite: true
   joint_group_names:
-    - gripper
-    - manipulator
+  - gripper
+  - manipulator
   joint_state:
     effort: []
     header:
@@ -315,19 +315,19 @@
         nanosec: 0
         sec: 0
     name:
-      - joint_1
-      - joint_2
-      - joint_3
-      - joint_4
-      - joint_5
-      - joint_6
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
     position:
-      - 1.1625409639061022
-      - 0.7569899884584295
-      - 0.2324359658075662
-      - -0.07323192405080868
-      - -0.9154444425780317
-      - 0.4408607192523566
+    - -1.5509811858997467
+    - 0.5153668404756869
+    - -0.7219805067810909
+    - 0.0013302776778533103
+    - -0.006224477478198248
+    - 3.1644727674098895e-08
     velocity: []
   multi_dof_joint_state:
     header:
@@ -339,4 +339,4 @@
     transforms: []
     twist: []
     wrench: []
-  name: Drop block
+  name: View shelf