diff --git a/src/example_behaviors/package.xml b/src/example_behaviors/package.xml
index 606865ad..0ac3ce5d 100644
--- a/src/example_behaviors/package.xml
+++ b/src/example_behaviors/package.xml
@@ -19,6 +19,7 @@
   <depend>moveit_ros_planning_interface</depend>
   <depend>moveit_studio_vision_msgs</depend>
   <depend>moveit_studio_vision</depend>
+  <depend>moveit_studio_behavior</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
diff --git a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
index 04947544..31a2650f 100644
--- a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
+++ b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
@@ -3,34 +3,39 @@
   <!--//////////-->
   <BehaviorTree ID="Add Waypoints to MTC Task">
     <Control ID="Sequence">
-      <Decorator
-        ID="ForEachString"
-        vector_in="{waypoint_names}"
-        out="{waypoint_name}"
-      >
-        <Control ID="Sequence">
-          <Action
-            ID="RetrieveWaypoint"
-            waypoint_joint_state="{target_joint_state}"
-            waypoint_name="{waypoint_name}"
-            joint_group_name="{joint_group_name}"
-          />
-          <Action
-            ID="SetupMTCMoveToJointState"
-            joint_state="{target_joint_state}"
-            name="SetupMTCMoveToJointState"
-            planning_group_name="{joint_group_name}"
-            planner_interface="{planner_interface}"
-            task="{mtc_task}"
-          />
-        </Control>
-      </Decorator>
+      <Action
+        ID="RetrieveWaypoint"
+        waypoint_joint_state="{target_joint_state}"
+        waypoint_name="Pick Cube"
+        joint_group_name="{joint_group_name}"
+      />
+      <Action
+        ID="SetupMTCMoveToJointState"
+        joint_state="{target_joint_state}"
+        name="SetupMTCMoveToJointState"
+        planning_group_name="{joint_group_name}"
+        planner_interface="{planner_interface}"
+        task="{mtc_task}"
+      />
+      <Action
+        ID="RetrieveWaypoint"
+        waypoint_joint_state="{target_joint_state}"
+        waypoint_name="Place Cube"
+        joint_group_name="{joint_group_name}"
+      />
+      <Action
+        ID="SetupMTCMoveToJointState"
+        joint_state="{target_joint_state}"
+        name="SetupMTCMoveToJointState"
+        planning_group_name="{joint_group_name}"
+        planner_interface="{planner_interface}"
+        task="{mtc_task}"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Add Waypoints to MTC Task">
       <input_port name="joint_group_name" default="manipulator" />
-      <input_port name="waypoint_names" default="Pick Cube;Place Cube" />
       <inout_port name="mtc_task" default="{mtc_task}" />
       <input_port name="planner_interface" default="moveit_default" />
     </SubTree>
diff --git a/src/lab_sim/objectives/apriltag_detection_config.yaml b/src/lab_sim/objectives/apriltag_detection_config.yaml
index f1a96237..7af7bf1c 100644
--- a/src/lab_sim/objectives/apriltag_detection_config.yaml
+++ b/src/lab_sim/objectives/apriltag_detection_config.yaml
@@ -3,7 +3,7 @@ DetectAprilTags:
   apriltag_size: 0.04
   max_hamming: 0
   n_threads: 1
-  quad_decimate: 1
-  quad_sigma: 0.1
-  refine_edges: false
+  quad_decimate: 2
+  quad_sigma: 0.0
+  refine_edges: true
   z_up: false
diff --git a/src/lab_sim/objectives/apriltag_pick_object.xml b/src/lab_sim/objectives/apriltag_pick_object.xml
index 00a6ca45..df3f814f 100644
--- a/src/lab_sim/objectives/apriltag_pick_object.xml
+++ b/src/lab_sim/objectives/apriltag_pick_object.xml
@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
   <!--//////////-->
   <BehaviorTree
@@ -8,7 +9,7 @@
     <Control ID="Sequence" name="TopLevelSequence">
       <SubTree ID="Look at table" _collapsed="true" />
       <Action ID="ClearSnapshot" />
-      <SubTree ID="Wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
       <Control ID="Sequence" name="Setup">
         <SubTree ID="Open Gripper" />
       </Control>
@@ -27,19 +28,39 @@
         topic_name="/wrist_camera/color"
         message_out="{image}"
       />
+      <SubTree
+        ID="Get AprilTag Pose from Image"
+        image="{image}"
+        camera_info="{camera_info}"
+        parameters="{parameters}"
+        _collapsed="true"
+        detection_pose="{detection_pose}"
+      />
       <Action
-        ID="DetectAprilTags"
+        ID="TransformPose"
+        output_pose="{view_pose}"
+        input_pose="{detection_pose}"
+        quaternion_xyzw="0;0;0;1"
+        translation_xyz="0;-0.1;-0.15"
+      />
+      <SubTree
+        ID="Plan Move To Pose"
+        target_pose="{view_pose}"
+        move_to_pose_solution="{move_to_pose_solution}"
+      />
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
+      <SubTree
+        ID="Get AprilTag Pose from Image"
         image="{image}"
         camera_info="{camera_info}"
         parameters="{parameters}"
-        detections="{detections}"
+        _collapsed="true"
+        detection_pose="{detection_pose}"
       />
-      <Action ID="GetDetectionPose" />
-      <Action ID="TransformPoseFrame" input_pose="{detection_pose}" />
       <Action
         ID="TransformPose"
         output_pose="{offset_pose}"
-        input_pose="{output_pose}"
+        input_pose="{detection_pose}"
         quaternion_xyzw="0;0;0;1"
         translation_xyz="0;0;-0.01"
       />
@@ -68,4 +89,7 @@
       <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" />
     </Control>
   </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Pick April Tag Labeled Object" />
+  </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/create_pose_vector.xml b/src/lab_sim/objectives/create_pose_vector.xml
index af7432b9..787f42ea 100644
--- a/src/lab_sim/objectives/create_pose_vector.xml
+++ b/src/lab_sim/objectives/create_pose_vector.xml
@@ -6,82 +6,100 @@
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="1;0.5;1"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="0.75;0.5;0.9"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="0.5;0.5;0.8"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="0.25;0.5;0.7"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="0;0.5;0.6"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="-0.25;0.5;0.7"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="-0.5;0.5;0.8"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="-0.75;0.5;0.9"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
         position_xyz="-1;0.5;1"
+        stamped_pose="{stamped_pose}"
       />
       <Action
         ID="AddPoseStampedToVector"
         vector="{target_poses}"
+        input="{stamped_pose}"
       />
     </Control>
   </BehaviorTree>
diff --git a/src/lab_sim/objectives/get_april_tag_pose_from_image.xml b/src/lab_sim/objectives/get_april_tag_pose_from_image.xml
new file mode 100644
index 00000000..789682be
--- /dev/null
+++ b/src/lab_sim/objectives/get_april_tag_pose_from_image.xml
@@ -0,0 +1,41 @@
+<root BTCPP_format="4" main_tree_to_execute="Get AprilTag Pose from Image">
+  <!-- ////////// -->
+  <BehaviorTree ID="Get AprilTag Pose from Image">
+    <Control ID="Sequence">
+      <!--Get tag pose estimate in world frame-->
+      <Action
+        ID="DetectAprilTags"
+        image="{image}"
+        camera_info="{camera_info}"
+        parameters="{parameters}"
+        detections="{detections}"
+      />
+      <Action
+        ID="GetImage"
+        topic_name="/wrist_camera/color"
+        message_out="{image}"
+      />
+      <Action
+        ID="DetectAprilTags"
+        image="{image}"
+        camera_info="{camera_info}"
+        parameters="{parameters}"
+        detections="{detections}"
+      />
+      <Action ID="GetDetectionPose" />
+      <Action
+        ID="TransformPoseFrame"
+        input_pose="{detection_pose}"
+        output_pose="{detection_pose}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Get AprilTag Pose from Image">
+      <input_port name="image" default="{image}" />
+      <input_port name="camera_info" default="{camera_info}" />
+      <input_port name="parameters" default="{parameters}" />
+      <output_port name="detection_pose" default="{detection_pose}" />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/pick_object.xml b/src/lab_sim/objectives/pick_object.xml
index 66dcbacd..2aa4018c 100644
--- a/src/lab_sim/objectives/pick_object.xml
+++ b/src/lab_sim/objectives/pick_object.xml
@@ -1,4 +1,3 @@
-<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Pick object">
   <!--//////////-->
   <BehaviorTree
@@ -15,6 +14,7 @@
           ID="CreateStampedPose"
           orientation_xyzw="0;1;0;0"
           position_xyz="0.01;.75;0.515"
+          stamped_pose="{stamped_pose}"
         />
         <Action
           ID="AddPoseStampedToVector"
diff --git a/src/lab_sim/objectives/pick_up_cube.xml b/src/lab_sim/objectives/pick_up_cube.xml
index e23c4dab..336abbae 100644
--- a/src/lab_sim/objectives/pick_up_cube.xml
+++ b/src/lab_sim/objectives/pick_up_cube.xml
@@ -1,13 +1,13 @@
-<?xml version="1.0" encoding="utf-8"?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Pick up Cube">
   <!--//////////-->
   <BehaviorTree ID="Pick up Cube" _description="" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Clear Snapshot" _collapsed="true"/>
-      <SubTree ID="Look at table" _collapsed="true"/>
+      <SubTree ID="Clear Snapshot" _collapsed="true" />
+      <SubTree ID="Look at table" _collapsed="true" />
       <!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
-      <SubTree ID="Wrist camera snapshot" _collapsed="true"/>
-      <SubTree ID="Pick object" _collapsed="false"/>
+      <SubTree ID="Wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Pick object" _collapsed="false" />
     </Control>
   </BehaviorTree>
 </root>
diff --git a/src/lab_sim/objectives/place_object.xml b/src/lab_sim/objectives/place_object.xml
index 61c14dc3..6ef0149e 100644
--- a/src/lab_sim/objectives/place_object.xml
+++ b/src/lab_sim/objectives/place_object.xml
@@ -13,6 +13,7 @@
           ID="CreateStampedPose"
           orientation_xyzw="0;1;0;0"
           position_xyz="0.01;.75;0.5.15"
+          stamped_pose="{stamped_pose}"
         />
         <Action
           ID="AddPoseStampedToVector"
diff --git a/src/lab_sim/objectives/plan_and_save_trajectory.xml b/src/lab_sim/objectives/plan_and_save_trajectory.xml
index 232e1f08..a3c28efd 100644
--- a/src/lab_sim/objectives/plan_and_save_trajectory.xml
+++ b/src/lab_sim/objectives/plan_and_save_trajectory.xml
@@ -32,7 +32,7 @@
       />
       <SubTree
         ID="Add Waypoints to MTC Task"
-        _collapsed="true"
+        _collapsed="false"
         mtc_task="{mtc_task}"
         waypoint_names="Pick Cube;Place Cube"
         joint_group_name="manipulator"
@@ -50,4 +50,7 @@
       />
     </Control>
   </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Plan and Save Trajectory" />
+  </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/plan_move_to_pose.xml b/src/lab_sim/objectives/plan_move_to_pose.xml
new file mode 100644
index 00000000..b5072d77
--- /dev/null
+++ b/src/lab_sim/objectives/plan_move_to_pose.xml
@@ -0,0 +1,36 @@
+<root BTCPP_format="4" main_tree_to_execute="Plan Move To Pose">
+  <!-- ////////// -->
+  <BehaviorTree ID="Plan Move To Pose">
+    <Control ID="Sequence">
+      <Action
+        ID="InitializeMTCTask"
+        task_id="move_to_pose"
+        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+        task="{move_to_pose_task}"
+      />
+      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
+      <Action
+        ID="SetupMTCMoveToPose"
+        ik_frame="grasp_link"
+        planning_group_name="manipulator"
+        target_pose="{target_pose}"
+        task="{move_to_pose_task}"
+        planner_interface="moveit_default"
+      />
+      <Action
+        ID="PlanMTCTask"
+        solution="{move_to_pose_solution}"
+        task="{move_to_pose_task}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Plan Move To Pose">
+      <input_port name="target_pose" default="{target_pose}" />
+      <output_port
+        name="move_to_pose_solution"
+        default="{move_to_pose_solution}"
+      />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/register_cad_part.xml b/src/lab_sim/objectives/register_cad_part.xml
index f43db67f..3a301798 100644
--- a/src/lab_sim/objectives/register_cad_part.xml
+++ b/src/lab_sim/objectives/register_cad_part.xml
@@ -8,6 +8,7 @@
         ID="CreateStampedPose"
         position_xyz="0.02;.75;0.58"
         orientation_xyzw="0;0;0;1"
+        stamped_pose="{stamped_pose}"
         name="Initial Guess"
       />
       <SubTree
diff --git a/src/lab_sim/objectives/wrist_snap.xml b/src/lab_sim/objectives/wrist_snap.xml
index 83c58612..7c04c0c4 100644
--- a/src/lab_sim/objectives/wrist_snap.xml
+++ b/src/lab_sim/objectives/wrist_snap.xml
@@ -1,6 +1,11 @@
-<root BTCPP_format="4" main_tree_to_execute="Wrist camera snapshot">
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute=" Take wrist camera snapshot">
   <!--//////////-->
-  <BehaviorTree ID="Wrist camera snapshot" _description="" _favorite="true">
+  <BehaviorTree
+    ID="Take wrist camera snapshot"
+    _description=""
+    _favorite="true"
+  >
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Do not clear snapshots first, so we can take multiple snapshots from different angles -->
       <Action ID="GetPointCloud" topic_name="/wrist_camera/points" />
@@ -8,6 +13,10 @@
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Wrist camera snapshot" />
+    <SubTree ID="Take wrist camera snapshot">
+      <MetadataFields>
+        <Metadata subcategory="Application - Basic Examples" />
+      </MetadataFields>
+    </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/picknik_accessories b/src/picknik_accessories
index 8297fd9e..18b1e9f5 160000
--- a/src/picknik_accessories
+++ b/src/picknik_accessories
@@ -1 +1 @@
-Subproject commit 8297fd9eaa9015a76827d25277bb3c52b84d469d
+Subproject commit 18b1e9f50f8f566b979e7251baf8744fa6387b29