diff --git a/src/factory_sim/objectives/plan_and_execute_with_orientation_samples.xml b/src/factory_sim/objectives/plan_and_execute_with_orientation_samples.xml
index 0b1dfd1d..c73710de 100644
--- a/src/factory_sim/objectives/plan_and_execute_with_orientation_samples.xml
+++ b/src/factory_sim/objectives/plan_and_execute_with_orientation_samples.xml
@@ -70,14 +70,22 @@
           controller_names="{controller_names}"
           task="{mtc_task}"
           trajectory_monitoring="false"
+          timeout="-1.000000"
         />
         <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
         <Action
-          ID="SetupMTCConnectWithTrajectory"
-          constraints="{constraints}"
-          planner_interface="pro_rrt"
-          planning_group_name="{joint_group_name}"
+          ID="SetupMTCConnectWithProRRT"
+          acceleration_scale_factor="1.000000"
+          keep_orientation="false"
+          keep_orientation_link_names="grasp_link"
+          keep_orientation_tolerance="0.100000"
+          link_padding="0.000000"
+          max_iterations="5000"
+          planning_group_name="manipulator"
+          seed="0"
           task="{mtc_task}"
+          trajectory_sampling_rate="100"
+          velocity_scale_factor="1.000000"
         />
         <Action
           ID="SetupMTCBatchPoseIK"
@@ -152,6 +160,7 @@
                     keep_orientation_tolerance="0.05"
                     link_padding="0.01"
                     velocity_scale_factor="1.0"
+                    seed="0"
                   />
                 </Control>
               </Decorator>
diff --git a/src/factory_sim/objectives/plan_and_execute_with_world_vertical_orientation.xml b/src/factory_sim/objectives/plan_and_execute_with_world_vertical_orientation.xml
index c55c05d8..5a1246e8 100644
--- a/src/factory_sim/objectives/plan_and_execute_with_world_vertical_orientation.xml
+++ b/src/factory_sim/objectives/plan_and_execute_with_world_vertical_orientation.xml
@@ -6,13 +6,23 @@
     _description="Free space plan to an approach pose then free space plan to a target pose with optional orientation constraints. With orientation constraints the gripper will maintain its orientation relative to the world vertical axis when moving from the approach to target pose."
   >
     <Control ID="Sequence" name="Plan to approach then target">
-      <Action ID="SwitchController" activate_controllers="{controller_names}" />
+      <Action
+        ID="SwitchController"
+        activate_controllers="{controller_names}"
+        activate_asap="true"
+        automatic_deactivation="true"
+        controller_list_action_name="/controller_manager/list_controllers"
+        controller_switch_action_name="/controller_manager/switch_controller"
+        strictness="1"
+        timeout="-1.000000"
+      />
       <Action
         ID="InitializeMTCTask"
         controller_names="{controller_names}"
         task="{mtc_task}"
         task_id="move_to_pose"
         trajectory_monitoring="false"
+        timeout="-1.000000"
       />
       <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
       <Action
@@ -94,6 +104,7 @@
                   keep_orientation_tolerance="0.05"
                   link_padding="0.01"
                   velocity_scale_factor="1.0"
+                  seed="0"
                 />
               </Control>
             </Decorator>
diff --git a/src/factory_sim/objectives/plan_path_along_surface.xml b/src/factory_sim/objectives/plan_path_along_surface.xml
index 1f985f2c..60a08a0e 100644
--- a/src/factory_sim/objectives/plan_path_along_surface.xml
+++ b/src/factory_sim/objectives/plan_path_along_surface.xml
@@ -121,11 +121,18 @@
         />
         <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
         <Action
-          ID="SetupMTCConnectWithTrajectory"
-          planner_interface="pro_rrt"
+          ID="SetupMTCConnectWithProRRT"
+          acceleration_scale_factor="1.000000"
+          keep_orientation="false"
+          keep_orientation_link_names="grasp_link"
+          keep_orientation_tolerance="0.100000"
+          link_padding="0.000000"
+          max_iterations="5000"
           planning_group_name="manipulator"
+          seed="0"
           task="{mtc_task}"
-          constraints="{constraints}"
+          trajectory_sampling_rate="100"
+          velocity_scale_factor="1.000000"
         />
         <Action
           ID="SetupMTCBatchPoseIK"
diff --git a/src/factory_sim/objectives/reachability_analysis_-_blocks.xml b/src/factory_sim/objectives/reachability_analysis_-_blocks.xml
index e35c8224..81685dd9 100644
--- a/src/factory_sim/objectives/reachability_analysis_-_blocks.xml
+++ b/src/factory_sim/objectives/reachability_analysis_-_blocks.xml
@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Reachability Analysis - blocks">
   <!--//////////-->
   <BehaviorTree
diff --git a/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml b/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
index 83ec7f83..cc7d4356 100644
--- a/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
+++ b/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
@@ -215,6 +215,7 @@
                       task="{task}"
                       task_id=""
                       trajectory_monitoring="false"
+                      timeout="-1.000000"
                     />
                     <Action ID="SetupMTCCurrentState" task="{task}" />
                     <Action
@@ -225,11 +226,18 @@
                       task="{task}"
                     />
                     <Action
-                      ID="SetupMTCConnectWithTrajectory"
-                      constraints="{constraints}"
-                      planner_interface="pro_rrt"
+                      ID="SetupMTCConnectWithProRRT"
+                      acceleration_scale_factor="1.000000"
+                      keep_orientation="false"
+                      keep_orientation_link_names="grasp_link"
+                      keep_orientation_tolerance="0.100000"
+                      link_padding="0.000000"
+                      max_iterations="5000"
                       planning_group_name="manipulator"
+                      seed="0"
                       task="{task}"
+                      trajectory_sampling_rate="100"
+                      velocity_scale_factor="1.000000"
                     />
                     <Action
                       ID="SetupMTCMoveAlongFrameAxis"
diff --git a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
index 739a409a..e6a0f2d6 100644
--- a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
+++ b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
@@ -13,11 +13,19 @@
         waypoint_name="Look at Table"
         joint_group_name="manipulator"
         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"
+        seed="0"
+        velocity_scale_factor="1.0"
       />
       <Action
         ID="MoveGripperAction"
         gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
         position="0"
+        timeout="10.000000"
       />
       <!--Run pick and place on loop-->
       <Decorator ID="KeepRunningUntilFailure">
@@ -29,34 +37,64 @@
               waypoint_name="Pick Cube"
               joint_group_name="manipulator"
               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"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
             <Action
               ID="MoveGripperAction"
               gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
               position="0.7929"
+              timeout="10.000000"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
               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"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
             <SubTree
               ID="Move to Waypoint"
               joint_group_name="manipulator"
               controller_names="joint_trajectory_controller"
               waypoint_name="Place Cube"
+              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"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
             <Action
               ID="MoveGripperAction"
               gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
               position="0"
+              timeout="10.000000"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
               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"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
           </Control>
           <!--Pick object from where it was placed, put it down on the original location, and go back to center pose-->
@@ -66,34 +104,64 @@
               waypoint_name="Place Cube"
               joint_group_name="manipulator"
               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"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
             <Action
               ID="MoveGripperAction"
               gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
               position="0.7929"
+              timeout="10.000000"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
               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"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Pick Cube"
               joint_group_name="manipulator"
               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"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
             <Action
               ID="MoveGripperAction"
               gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
               position="0"
+              timeout="10.000000"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
               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"
+              seed="0"
+              velocity_scale_factor="1.0"
             />
           </Control>
         </Control>
diff --git a/src/lab_sim/objectives/grasp_planning.xml b/src/lab_sim/objectives/grasp_planning.xml
index 04d3bea2..0014eff0 100644
--- a/src/lab_sim/objectives/grasp_planning.xml
+++ b/src/lab_sim/objectives/grasp_planning.xml
@@ -62,14 +62,22 @@
           controller_names="joint_trajectory_controller;robotiq_gripper_controller"
           task="{pick_object_task}"
           trajectory_monitoring="false"
+          timeout="-1.000000"
         />
         <Action ID="SetupMTCCurrentState" task="{pick_object_task}" />
         <Action
-          ID="SetupMTCConnectWithTrajectory"
-          constraints="{constraints}"
-          planner_interface="pro_rrt"
+          ID="SetupMTCConnectWithProRRT"
+          acceleration_scale_factor="1.000000"
+          keep_orientation="false"
+          keep_orientation_link_names="grasp_link"
+          keep_orientation_tolerance="0.100000"
+          link_padding="0.000000"
+          max_iterations="5000"
           planning_group_name="manipulator"
+          seed="0"
           task="{pick_object_task}"
+          trajectory_sampling_rate="100"
+          velocity_scale_factor="1.000000"
         />
         <Action
           ID="SetupMTCBatchPoseIK"
@@ -90,6 +98,7 @@
         <SubTree
           ID="Wait for Trajectory Approval if User Available"
           solution="{pick_object_solution}"
+          cartesian_path_links="grasp_link"
         />
         <Action
           ID="ExecuteMTCTask"
diff --git a/src/lab_sim/objectives/ml_grasp_object_from_text_prompt.xml b/src/lab_sim/objectives/ml_grasp_object_from_text_prompt.xml
index 2100fc12..a1d31388 100644
--- a/src/lab_sim/objectives/ml_grasp_object_from_text_prompt.xml
+++ b/src/lab_sim/objectives/ml_grasp_object_from_text_prompt.xml
@@ -74,14 +74,22 @@
           controller_names="joint_trajectory_controller;robotiq_gripper_controller"
           task="{mtc_task}"
           trajectory_monitoring="false"
+          timeout="-1.000000"
         />
         <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
         <Action
-          ID="SetupMTCConnectWithTrajectory"
-          planner_interface="pro_rrt"
+          ID="SetupMTCConnectWithProRRT"
+          acceleration_scale_factor="1.000000"
+          keep_orientation="false"
+          keep_orientation_link_names="grasp_link"
+          keep_orientation_tolerance="0.100000"
+          link_padding="0.000000"
+          max_iterations="5000"
           planning_group_name="manipulator"
+          seed="0"
           task="{mtc_task}"
-          constraints="{constraints}"
+          trajectory_sampling_rate="100"
+          velocity_scale_factor="1.000000"
         />
         <Action
           ID="SetupMTCBatchPoseIK"
diff --git a/src/lab_sim/objectives/pick_and_place_example.xml b/src/lab_sim/objectives/pick_and_place_example.xml
index 428aa2ee..6d2ad6b2 100644
--- a/src/lab_sim/objectives/pick_and_place_example.xml
+++ b/src/lab_sim/objectives/pick_and_place_example.xml
@@ -21,11 +21,15 @@
           ID="Pick from Pose"
           _collapsed="true"
           grasp_pose="{pick_place_pose}"
+          approach_distance="0.1"
+          retract_xyz="0;0;-0.1"
         />
         <SubTree
           ID="Place at Pose"
           _collapsed="true"
           place_pose="{pick_place_pose}"
+          approach_distance="0.1"
+          retract_xyz="0;0;-0.1"
         />
       </Control>
     </Control>
diff --git a/src/lab_sim/objectives/pick_from_pose.xml b/src/lab_sim/objectives/pick_from_pose.xml
index d1d2b676..831987eb 100644
--- a/src/lab_sim/objectives/pick_from_pose.xml
+++ b/src/lab_sim/objectives/pick_from_pose.xml
@@ -13,14 +13,22 @@
         task="{mtc_task}"
         task_id="pick_object"
         trajectory_monitoring="false"
+        timeout="-1.000000"
       />
       <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
       <Action
-        ID="SetupMTCConnectWithTrajectory"
-        planner_interface="pro_rrt"
+        ID="SetupMTCConnectWithProRRT"
+        acceleration_scale_factor="1.000000"
+        keep_orientation="false"
+        keep_orientation_link_names="grasp_link"
+        keep_orientation_tolerance="0.100000"
+        link_padding="0.000000"
+        max_iterations="5000"
         planning_group_name="manipulator"
+        seed="0"
         task="{mtc_task}"
-        constraints="{constraints}"
+        trajectory_sampling_rate="100"
+        velocity_scale_factor="1.000000"
       />
       <Action
         ID="SetupMTCMoveAlongFrameAxis"
@@ -95,6 +103,7 @@
         goal_position_tolerance="0.000000"
         goal_time_tolerance="0.000000"
         joint_trajectory_msg="{joint_trajectory_msg}"
+        trajectory_remainder="{joint_trajectory_remainder}"
       />
     </Control>
   </BehaviorTree>
diff --git a/src/lab_sim/objectives/pick_from_pose_with_approval.xml b/src/lab_sim/objectives/pick_from_pose_with_approval.xml
index 9f64bbce..40e100e2 100644
--- a/src/lab_sim/objectives/pick_from_pose_with_approval.xml
+++ b/src/lab_sim/objectives/pick_from_pose_with_approval.xml
@@ -13,14 +13,22 @@
         task="{mtc_task}"
         task_id="pick_object"
         trajectory_monitoring="false"
+        timeout="-1.000000"
       />
       <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
       <Action
-        ID="SetupMTCConnectWithTrajectory"
-        planner_interface="pro_rrt"
+        ID="SetupMTCConnectWithProRRT"
+        acceleration_scale_factor="1.000000"
+        keep_orientation="false"
+        keep_orientation_link_names="grasp_link"
+        keep_orientation_tolerance="0.100000"
+        link_padding="0.000000"
+        max_iterations="5000"
         planning_group_name="manipulator"
+        seed="0"
         task="{mtc_task}"
-        constraints="{constraints}"
+        trajectory_sampling_rate="100"
+        velocity_scale_factor="1.000000"
       />
       <Action
         ID="SetupMTCMoveAlongFrameAxis"
@@ -59,6 +67,7 @@
         ID="Wait for Trajectory Approval if User Available"
         _collapsed="false"
         solution="{mtc_solution}"
+        cartesian_path_links="grasp_link"
       />
       <Action
         ID="ExecuteMTCTask"
diff --git a/src/lab_sim/objectives/place_at_pose.xml b/src/lab_sim/objectives/place_at_pose.xml
index a22e73f0..b0270a3a 100644
--- a/src/lab_sim/objectives/place_at_pose.xml
+++ b/src/lab_sim/objectives/place_at_pose.xml
@@ -13,14 +13,22 @@
         task="{mtc_task}"
         task_id="place_object"
         trajectory_monitoring="false"
+        timeout="-1.000000"
       />
       <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
       <Action
-        ID="SetupMTCConnectWithTrajectory"
-        planner_interface="pro_rrt"
+        ID="SetupMTCConnectWithProRRT"
+        acceleration_scale_factor="1.000000"
+        keep_orientation="false"
+        keep_orientation_link_names="grasp_link"
+        keep_orientation_tolerance="0.100000"
+        link_padding="0.000000"
+        max_iterations="5000"
         planning_group_name="manipulator"
+        seed="0"
         task="{mtc_task}"
-        constraints="{constraints}"
+        trajectory_sampling_rate="100"
+        velocity_scale_factor="1.000000"
       />
       <Action
         ID="SetupMTCMoveAlongFrameAxis"
@@ -95,6 +103,7 @@
         goal_position_tolerance="0.000000"
         goal_time_tolerance="0.000000"
         joint_trajectory_msg="{joint_trajectory_msg}"
+        trajectory_remainder="{joint_trajectory_remainder}"
       />
     </Control>
   </BehaviorTree>
diff --git a/src/lab_sim/objectives/place_at_pose_with_approval.xml b/src/lab_sim/objectives/place_at_pose_with_approval.xml
index 78e33173..4c27457f 100644
--- a/src/lab_sim/objectives/place_at_pose_with_approval.xml
+++ b/src/lab_sim/objectives/place_at_pose_with_approval.xml
@@ -13,14 +13,22 @@
         task="{mtc_task}"
         task_id="place_object"
         trajectory_monitoring="false"
+        timeout="-1.000000"
       />
       <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
       <Action
-        ID="SetupMTCConnectWithTrajectory"
-        planner_interface="pro_rrt"
+        ID="SetupMTCConnectWithProRRT"
+        acceleration_scale_factor="1.000000"
+        keep_orientation="false"
+        keep_orientation_link_names="grasp_link"
+        keep_orientation_tolerance="0.100000"
+        link_padding="0.000000"
+        max_iterations="5000"
         planning_group_name="manipulator"
+        seed="0"
         task="{mtc_task}"
-        constraints="{constraints}"
+        trajectory_sampling_rate="100"
+        velocity_scale_factor="1.000000"
       />
       <Action
         ID="SetupMTCMoveAlongFrameAxis"
@@ -53,6 +61,7 @@
         ID="Wait for Trajectory Approval if User Available"
         _collapsed="true"
         solution="{mtc_solution}"
+        cartesian_path_links="grasp_link"
       />
       <Action
         ID="ExecuteMTCTask"
@@ -93,6 +102,7 @@
         goal_position_tolerance="0.000000"
         goal_time_tolerance="0.000000"
         joint_trajectory_msg="{joint_trajectory_msg}"
+        trajectory_remainder="{joint_trajectory_remainder}"
       />
     </Control>
   </BehaviorTree>
@@ -102,7 +112,6 @@
         <Metadata subcategory="Application - Basic Examples" />
         <Metadata runnable="false" />
       </MetadataFields>
-
       <inout_port name="place_poses" default="{place_poses}" />
     </SubTree>
   </TreeNodesModel>