diff --git a/src/lab_sim/objectives/_scan_scene.xml b/src/lab_sim/objectives/_scan_scene.xml
index 1a345139..30117567 100644
--- a/src/lab_sim/objectives/_scan_scene.xml
+++ b/src/lab_sim/objectives/_scan_scene.xml
@@ -3,7 +3,7 @@
   <!--//////////-->
   <BehaviorTree
     ID="_Scan Scene"
-    _description="Move robot our of scene to enable full snapshot of scene."
+    _description="Move robot out of scene to enable full snapshot of scene."
     _favorite="true"
   >
     <Control ID="Sequence" name="TopLevelSequence">
diff --git a/src/lab_sim/objectives/constrained_pick_place.xml b/src/lab_sim/objectives/constrained_pick_place.xml
index 405f8e64..4d6f3e8e 100644
--- a/src/lab_sim/objectives/constrained_pick_place.xml
+++ b/src/lab_sim/objectives/constrained_pick_place.xml
@@ -49,7 +49,7 @@
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Move Beakers to Burners" />
+    <SubTree ID="Move Beakers to Burners">
       <MetadataFields>
         <Metadata subcategory="Application - Advanced Examples" />
       </MetadataFields>
diff --git a/src/lab_sim/objectives/create_pose_vector.xml b/src/lab_sim/objectives/create_pose_vector.xml
index 661b04b3..6ac88526 100644
--- a/src/lab_sim/objectives/create_pose_vector.xml
+++ b/src/lab_sim/objectives/create_pose_vector.xml
@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Create Pose Vector">
   <!--//////////-->
   <BehaviorTree
@@ -5,13 +6,15 @@
     _favorite="false"
     _subtreeOnly="true"
     _description=""
+    target_poses="{target_poses}"
   >
     <Control ID="Sequence">
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
-        position_xyz="1;0.5;1"
+        position_xyz="0.75;0.5;0.9"
         stamped_pose="{stamped_pose}"
+        reference_frame="world"
       />
       <Action
         ID="AddPoseStampedToVector"
@@ -23,6 +26,31 @@
         orientation_xyzw="0;-1;0;0"
         position_xyz="0.5;0.5;0.8"
         stamped_pose="{stamped_pose}"
+        reference_frame="world"
+      />
+      <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}"
+        reference_frame="world"
+      />
+      <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}"
+        reference_frame="world"
       />
       <Action
         ID="AddPoseStampedToVector"
@@ -34,6 +62,31 @@
         orientation_xyzw="0;-1;0;0"
         position_xyz="-0.25;0.5;0.7"
         stamped_pose="{stamped_pose}"
+        reference_frame="world"
+      />
+      <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}"
+        reference_frame="world"
+      />
+      <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}"
+        reference_frame="world"
       />
       <Action
         ID="AddPoseStampedToVector"
@@ -45,6 +98,7 @@
         orientation_xyzw="0;-1;0;0"
         position_xyz="-1;0.5;1"
         stamped_pose="{stamped_pose}"
+        reference_frame="world"
       />
       <Action
         ID="AddPoseStampedToVector"
@@ -58,6 +112,7 @@
       <output_port name="target_poses" default="{target_poses}" />
       <MetadataFields>
         <Metadata subcategory="Application - Basic Examples" />
+        <Metadata runnable="false" />
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml b/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml
index b8ddbe4d..1217c4dd 100644
--- a/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml
+++ b/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml
@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Load and Execute Joint Trajectory">
   <!--//////////-->
   <BehaviorTree
@@ -7,7 +8,7 @@
     _subtreeOnly="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
-      <!-- Make sure to move the robot to the same starting position as the saved trajectory starts, otherwise the joint_trajectory_controller will use joint interpolation to align the robot with the path -->
+      <!--Make sure to move the robot to the same starting position as the saved trajectory starts, otherwise the joint_trajectory_controller will use joint interpolation to align the robot with the path-->
       <SubTree
         ID="Move to Waypoint"
         _collapsed="true"
@@ -15,18 +16,47 @@
         joint_group_name="manipulator"
         planner_interface="moveit_default"
         controller_names="/joint_trajectory_controller"
+        acceleration_scale_factor="1.0"
+        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+        keep_orientation="false"
+        keep_orientation_tolerance="0.05"
+        link_padding="0.01"
+        velocity_scale_factor="1.0"
       />
       <Action
         ID="LoadJointTrajectoryFromYaml"
         file_path="joint_trajectory.yaml"
+        output="{joint_trajectory_msg}"
+      />
+      <!--Validate the loaded trajectory with the current planning scene to check for collisions before executing-->
+      <Action
+        ID="GetCurrentPlanningScene"
+        planning_scene_msg="{planning_scene}"
+      />
+      <Action
+        ID="ValidateTrajectory"
+        cartesian_space_step="0.020000"
+        joint_space_step="0.100000"
+        joint_trajectory_msg="{joint_trajectory_msg}"
+        planning_group_name="manipulator"
+        planning_scene_msg="{planning_scene}"
+        debug_solution="{debug_solution}"
+      />
+      <Action
+        ID="ExecuteFollowJointTrajectory"
+        execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
+        goal_duration_tolerance="-1.000000"
+        goal_position_tolerance="0.000000"
+        goal_time_tolerance="0.000000"
+        joint_trajectory_msg="{joint_trajectory_msg}"
       />
-      <Action ID="ExecuteFollowJointTrajectory" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Load and Execute Joint Trajectory">
       <MetadataFields>
         <Metadata subcategory="Application - Basic Examples" />
+        <Metadata runnable="true" />
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/plan_and_save_trajectory.xml b/src/lab_sim/objectives/plan_and_save_trajectory.xml
index 2cc64254..97027995 100644
--- a/src/lab_sim/objectives/plan_and_save_trajectory.xml
+++ b/src/lab_sim/objectives/plan_and_save_trajectory.xml
@@ -8,7 +8,7 @@
     _subtreeOnly="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
-      <!-- Before we plan we want to make sure we are at a known position, so when we execute the plan it can be done from the same position -->
+      <!--Before we plan we want to make sure we are at a known position, so when we execute the plan it can be done from the same position-->
       <SubTree
         ID="Move to Waypoint"
         _collapsed="true"
@@ -23,45 +23,30 @@
         link_padding="0.01"
         velocity_scale_factor="1.0"
       />
-      <!-- Create a vector of poses so we can add them all to the MTC task at once -->
+      <!--Create a vector of poses we want to move through-->
       <SubTree
         ID="Create Pose Vector"
         _collapsed="false"
         target_poses="{target_poses}"
       />
-      <!-- Add the cartesian poses we created in the previous step -->
-      <SubTree
-        ID="Add Poses to MTC Task"
-        _collapsed="true"
-        target_poses="{target_poses}"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planning_group_name="manipulator"
-        ik_frame="grasp_link"
-        mtc_task="{mtc_task}"
-      />
-      <!-- Add joint-state waypoints to the MTC task as well -->
-      <SubTree
-        ID="Add Waypoints to MTC Task"
-        _collapsed="false"
-        mtc_task="{mtc_task}"
-        waypoint_names="Pick Cube;Place Cube"
-        joint_group_name="manipulator"
-        planner_interface="moveit_default"
-      />
-      <!-- Perform the computationally intensive calculation now -->
-      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
-      <!-- Preview the trajectory for approval -->
-      <Action ID="WaitForUserTrajectoryApproval" solution="{mtc_solution}" />
-      <!-- The MTC solution does not contain time parameterization, so we can use TOTG to convert it to a JointTrajectory. The source code for the following behavior can be found in the example_behaviors package -->
+      <!--Plan a cartesian path through the desired poses-->
       <Action
-        ID="ConvertMtcSolutionToJointTrajectory"
-        joint_trajectory="{joint_trajectory_msg}"
-        acceleration_scaling_factor="1.000000"
-        joint_group="manipulator"
-        sampling_rate="100"
-        solution="{mtc_solution}"
-        velocity_scaling_factor="1.000000"
+        ID="PlanCartesianPath"
+        acceleration_scale_factor="1.000000"
+        blending_radius="0.020000"
+        ik_cartesian_space_density="0.010000"
+        ik_joint_space_density="0.100000"
+        path="{target_poses}"
+        planning_group_name="manipulator"
+        position_only="false"
+        tip_link="grasp_link"
+        trajectory_sampling_rate="100"
+        velocity_scale_factor="1.000000"
+        debug_solution="{debug_solution}"
+        joint_trajectory_msg="{joint_trajectory_msg}"
       />
+      <!--Preview the trajectory for approval and save if approved-->
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
       <Action
         ID="SaveJointTrajectoryToYaml"
         yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"
diff --git a/src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml b/src/lab_sim/objectives/record_and_replay_scanning_motion.xml
similarity index 100%
rename from src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml
rename to src/lab_sim/objectives/record_and_replay_scanning_motion.xml
diff --git a/src/lab_sim/waypoints/ur_waypoints.yaml b/src/lab_sim/waypoints/ur_waypoints.yaml
index d4fc1c23..e2b30d54 100644
--- a/src/lab_sim/waypoints/ur_waypoints.yaml
+++ b/src/lab_sim/waypoints/ur_waypoints.yaml
@@ -901,3 +901,46 @@
     twist: []
     wrench: []
   name: Beaker 2a - Pre Pick
+- description: ''
+  favorite: false
+  joint_group_names:
+    - gripper
+    - linear_actuator
+    - manipulator
+  joint_state:
+    effort: []
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+      - robotiq_85_left_knuckle_joint
+      - linear_rail_joint
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    position:
+      - 0.06656709575770764
+      - 0.2544520984236789
+      - 0.00297097958947393
+      - -0.5454836524482858
+      - 1.028930149174636
+      - -2.02088483587436
+      - -1.5709145918870968
+      - 0.002734726536987722
+    velocity: []
+  multi_dof_joint_state:
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    joint_names: []
+    transforms: []
+    twist: []
+    wrench: []
+  name: Pick Cube
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/waypoints/ur_waypoints.yaml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/waypoints/ur_waypoints.yaml
index 76c7d51a..9b91926d 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/waypoints/ur_waypoints.yaml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/waypoints/ur_waypoints.yaml
@@ -1,9 +1,9 @@
 - description: ''
   favorite: true
   joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
+    - gripper
+    - linear_actuator
+    - manipulator
   joint_state:
     effort: []
     header:
diff --git a/src/moveit_pro_ur_configs/arm_on_rail_sim/waypoints/ur_waypoints.yaml b/src/moveit_pro_ur_configs/arm_on_rail_sim/waypoints/ur_waypoints.yaml
index 0fd27077..9cdcceb1 100644
--- a/src/moveit_pro_ur_configs/arm_on_rail_sim/waypoints/ur_waypoints.yaml
+++ b/src/moveit_pro_ur_configs/arm_on_rail_sim/waypoints/ur_waypoints.yaml
@@ -1,9 +1,9 @@
 - description: ''
   favorite: false
   joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
+    - gripper
+    - linear_actuator
+    - manipulator
   joint_state:
     effort: []
     header: