From 3084b0ec83aa189032136b9bee23869f58fa9f1f Mon Sep 17 00:00:00 2001
From: Mario Prats <mario.prats@picknik.ai>
Date: Wed, 18 Dec 2024 14:16:38 +0100
Subject: [PATCH] Make Pro-RRT the default in all configs

---
 .../objectives/move_to_joint_state.xml        |  44 +--
 src/fanuc_sim/objectives/move_to_waypoint.xml |  47 ++--
 src/lab_sim/config/moveit/picknik_ur.srdf     |   3 +
 .../objectives/3_waypoint_pick_and_place.xml  |  36 +--
 .../objectives/constrained_pick_place.xml     | 254 +++++++++---------
 .../objectives/cycle_between_waypoints.xml    |   8 +-
 .../move_with_velocity_and_force.xml          |   1 -
 src/lab_sim/objectives/my_constraints.yaml    |  11 -
 .../objectives/move_to_joint_state.xml        |  84 +++---
 .../objectives/move_to_waypoint.xml           |  50 ++--
 .../objectives/3_waypoint_pick_and_place.xml  |  36 +--
 .../objectives/compliant_grasp_landsat.xml    |   2 +-
 .../objectives/compliant_grasp_rafti.xml      |   2 +-
 .../objectives/compliant_grasp_rafti_vfc.xml  |   3 +-
 .../objectives/cycle_between_waypoints.xml    |   4 +-
 .../objectives/joint_diagnostic.xml           |   6 +-
 .../objectives/move_to_joint_state.xml        |  84 +++---
 .../objectives/move_to_waypoint.xml           |  43 ++-
 .../objectives/move_to_joint_state.xml        |  44 +--
 .../objectives/move_to_waypoint.xml           |  43 ++-
 .../objectives/move_to_joint_state.xml        |  45 ++--
 .../objectives/move_to_named_pose.xml         |  38 ---
 .../objectives/move_to_waypoint.xml           |  52 ++++
 .../objectives/move_two_arms.xml              |  51 ++--
 24 files changed, 491 insertions(+), 500 deletions(-)
 delete mode 100644 src/lab_sim/objectives/my_constraints.yaml
 delete mode 100644 src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_named_pose.xml
 create mode 100644 src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_waypoint.xml

diff --git a/src/fanuc_sim/objectives/move_to_joint_state.xml b/src/fanuc_sim/objectives/move_to_joint_state.xml
index 63ab4a8e..08e507c2 100644
--- a/src/fanuc_sim/objectives/move_to_joint_state.xml
+++ b/src/fanuc_sim/objectives/move_to_joint_state.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
   <BehaviorTree
     ID="Move to Joint State"
@@ -6,32 +6,40 @@
     _subtreeOnly="true"
   >
     <Control ID="Sequence">
+      <Action ID="ActivateControllers" controller_names="{controller_names}" />
       <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_joint_state"
-        controller_names="/joint_trajectory_controller"
-        task="{move_to_waypoint_task}"
+        ID="PlanToJointGoal"
+        joint_goal="{target_joint_state}"
+        keep_orientation_tolerance="{keep_orientation_tolerance}"
+        link_padding="{link_padding}"
+        keep_orientation="{keep_orientation}"
+        acceleration_scale_factor="{acceleration_scale_factor}"
+        velocity_scale_factor="{velocity_scale_factor}"
+        planning_group_name="{joint_group_name}"
       />
-      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
       <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCMoveToJointState_First"
-        planning_group_name="manipulator"
-        task="{move_to_waypoint_task}"
-        planner_interface="moveit_default"
+        ID="ExecuteFollowJointTrajectory"
+        execute_follow_joint_trajectory_action_name="{controller_action_server}"
       />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_waypoint_solution}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Joint State">
       <input_port name="target_joint_state" default="{target_joint_state}" />
+      <input_port name="joint_group_name" default="manipulator" />
+      <input_port name="link_padding" default="0.01" />
+      <input_port name="keep_orientation_tolerance" default="0.05" />
+      <input_port name="keep_orientation" default="false" />
+      <input_port name="velocity_scale_factor" default="1.0" />
+      <input_port name="acceleration_scale_factor" default="1.0" />
+      <input_port
+        name="controller_names"
+        default="joint_trajectory_controller"
+      />
+      <input_port
+        name="controller_action_server"
+        default="/joint_trajectory_controller/follow_joint_trajectory"
+      />
       <MetadataFields>
         <Metadata subcategory="Motion - Execute" />
       </MetadataFields>
diff --git a/src/fanuc_sim/objectives/move_to_waypoint.xml b/src/fanuc_sim/objectives/move_to_waypoint.xml
index ad9ec51d..3a8f285c 100644
--- a/src/fanuc_sim/objectives/move_to_waypoint.xml
+++ b/src/fanuc_sim/objectives/move_to_waypoint.xml
@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
   <BehaviorTree
     ID="Move to Waypoint"
@@ -11,42 +12,38 @@
         waypoint_name="{waypoint_name}"
         joint_group_name="{joint_group_name}"
       />
-      <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_named_pose"
+      <SubTree
+        ID="Move to Joint State"
+        _collapsed="true"
+        velocity_scale_factor="{velocity_scale_factor}"
+        acceleration_scale_factor="{acceleration_scale_factor}"
+        link_padding="{link_padding}"
+        keep_orientation_tolerance="{keep_orientation_tolerance}"
+        keep_orientation="{keep_orientation}"
+        target_joint_state="{target_joint_state}"
         controller_names="{controller_names}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
-      <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCMoveToJointState"
-        planning_group_name="{joint_group_name}"
-        planner_interface="{planner_interface}"
-        constraints="{constraints}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_waypoint_solution}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action
-        ID="BlockingExecuteMTCTask"
-        solution="{move_to_waypoint_solution}"
+        joint_group_name="{joint_group_name}"
+        controller_action_server="{controller_action_server}"
       />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Waypoint">
-      <input_port name="waypoint_name" default="Point A" />
+      <input_port name="waypoint_name" default="" />
       <input_port name="joint_group_name" default="manipulator" />
-      <input_port name="planner_interface" default="moveit_default" />
+      <input_port name="link_padding" default="0.01" />
+      <input_port name="keep_orientation_tolerance" default="0.05" />
+      <input_port name="keep_orientation" default="false" />
+      <input_port name="velocity_scale_factor" default="1.0" />
+      <input_port name="acceleration_scale_factor" default="1.0" />
       <input_port
         name="controller_names"
         default="/joint_trajectory_controller"
       />
+      <input_port
+        name="controller_action_server"
+        default="/joint_trajectory_controller/follow_joint_trajectory"
+      />
       <MetadataFields>
         <Metadata subcategory="Motion - Execute" />
       </MetadataFields>
diff --git a/src/lab_sim/config/moveit/picknik_ur.srdf b/src/lab_sim/config/moveit/picknik_ur.srdf
index c24efd02..f0440417 100644
--- a/src/lab_sim/config/moveit/picknik_ur.srdf
+++ b/src/lab_sim/config/moveit/picknik_ur.srdf
@@ -195,4 +195,7 @@
     <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent"/>
     <disable_collisions link1="wrist_2_link" link2="wrist_mounted_camera_link" reason="Never"/>
     <disable_collisions link1="wrist_3_link" link2="wrist_mounted_camera_link" reason="Never"/>
+    <disable_collisions link1="robotiq_85_right_finger_link" link2="robotiq_85_right_finger_tip_link" reason="Never"/>
+    <disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_left_finger_tip_link" reason="Never"/>
+    <disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Never"/>
 </robot>
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 60490ad4..3d94cc40 100644
--- a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
+++ b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
@@ -13,9 +13,7 @@
         ID="Move to Waypoint"
         waypoint_name="Look at Table"
         joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
-        constraints="{constraints}"
+        controller_names="/joint_trajectory_controller"
       />
       <Action
         ID="MoveGripperAction"
@@ -31,9 +29,7 @@
               ID="Move to Waypoint"
               waypoint_name="Pick Cube"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <Action
               ID="MoveGripperAction"
@@ -44,16 +40,12 @@
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <SubTree
               ID="Move to Waypoint"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
               waypoint_name="Place Cube"
             />
             <Action
@@ -65,9 +57,7 @@
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
           </Control>
           <!--Pick object from where it was placed, put it down on the original location, and go back to center pose-->
@@ -76,9 +66,7 @@
               ID="Move to Waypoint"
               waypoint_name="Place Cube"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <Action
               ID="MoveGripperAction"
@@ -89,17 +77,13 @@
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Pick Cube"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <Action
               ID="MoveGripperAction"
@@ -110,9 +94,7 @@
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
           </Control>
         </Control>
diff --git a/src/lab_sim/objectives/constrained_pick_place.xml b/src/lab_sim/objectives/constrained_pick_place.xml
index 838533b1..59a242f2 100644
--- a/src/lab_sim/objectives/constrained_pick_place.xml
+++ b/src/lab_sim/objectives/constrained_pick_place.xml
@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Constrained Pick and Place">
   <!--//////////-->
   <BehaviorTree
@@ -9,154 +10,149 @@
       <SubTree
         ID="Move to Waypoint"
         _collapsed="true"
-        constraints="{constraints}"
         waypoint_name="Look at Table"
         joint_group_name="manipulator"
-        planner_interface="moveit_default"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+        controller_names="/joint_trajectory_controller"
+        velocity_scale_factor="1.0"
+        acceleration_scale_factor="1.0"
+        link_padding="0.01"
+        keep_orientation_tolerance="0.05"
+        keep_orientation="false"
       />
       <SubTree ID="Open Gripper" />
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
-          <!--Create a special type of motion planning configuration that includes an upwards orientation requirement-->
-          <Action
-            ID="InitializeMotionConstraints"
-            constraints_name="gripper pointing down"
-            constraints="{constraints}"
+          <!--Move to pick location-->
+          <SubTree
+            ID="Move to Waypoint"
+            waypoint_name="Above Pick Cube"
+            joint_group_name="manipulator"
+            controller_names="/joint_trajectory_controller"
+            velocity_scale_factor="1.0"
+            acceleration_scale_factor="1.0"
+            link_padding="0.01"
+            keep_orientation_tolerance="0.05"
+            keep_orientation="true"
           />
-          <Action
-            ID="AppendOrientationConstraint"
-            config_file_name="my_constraints.yaml"
-            constraints="{constraints}"
+          <SubTree
+            ID="Move to Waypoint"
+            waypoint_name="Pick Cube"
+            joint_group_name="manipulator"
+            controller_names="/joint_trajectory_controller"
+            velocity_scale_factor="1.0"
+            acceleration_scale_factor="1.0"
+            link_padding="0.01"
+            keep_orientation_tolerance="0.05"
+            keep_orientation="true"
           />
-          <!--Move to pick location-->
-          <!--Note: Sampling based planners can be non-deterministic. The retry decorator improves the likelihood of success-->
-          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Above Pick Cube"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
-            />
-          </Decorator>
-          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Pick Cube"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
-            />
-          </Decorator>
           <!--We force success as the gripper closes, since we are commanding a position it will never reach (fingers fully closed)-->
           <Decorator ID="ForceSuccess">
             <SubTree ID="Close Gripper" />
           </Decorator>
           <!--Move to place (drop) location-->
-          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <SubTree
-              ID="Move to Waypoint"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              joint_group_name="manipulator"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
-              _collapsed="true"
-              waypoint_name="Above Pick Cube"
-            />
-          </Decorator>
-          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <SubTree
-              ID="Move to Waypoint"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              joint_group_name="manipulator"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
-              _collapsed="true"
-              waypoint_name="Above Place Cube"
-            />
-          </Decorator>
-          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <SubTree
-              ID="Move to Waypoint"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              joint_group_name="manipulator"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
-              _collapsed="true"
-              waypoint_name="Place Cube"
-            />
-          </Decorator>
+          <SubTree
+            ID="Move to Waypoint"
+            controller_names="/joint_trajectory_controller"
+            joint_group_name="manipulator"
+            _collapsed="true"
+            waypoint_name="Above Pick Cube"
+            velocity_scale_factor="1.0"
+            acceleration_scale_factor="1.0"
+            link_padding="0.01"
+            keep_orientation_tolerance="0.05"
+            keep_orientation="true"
+          />
+          <SubTree
+            ID="Move to Waypoint"
+            controller_names="/joint_trajectory_controller"
+            joint_group_name="manipulator"
+            _collapsed="true"
+            waypoint_name="Above Place Cube"
+            velocity_scale_factor="1.0"
+            acceleration_scale_factor="1.0"
+            link_padding="0.01"
+            keep_orientation_tolerance="0.05"
+            keep_orientation="true"
+          />
+          <SubTree
+            ID="Move to Waypoint"
+            controller_names="/joint_trajectory_controller"
+            joint_group_name="manipulator"
+            _collapsed="true"
+            waypoint_name="Place Cube"
+            velocity_scale_factor="1.0"
+            acceleration_scale_factor="1.0"
+            link_padding="0.01"
+            keep_orientation_tolerance="0.05"
+            keep_orientation="true"
+          />
           <SubTree ID="Open Gripper" />
-          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <SubTree
-              ID="Move to Waypoint"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              joint_group_name="manipulator"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
-              _collapsed="true"
-              waypoint_name="Above Place Cube"
-            />
-          </Decorator>
-          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <SubTree
-              ID="Move to Waypoint"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              joint_group_name="manipulator"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
-              _collapsed="true"
-              waypoint_name="Place Cube"
-            />
-          </Decorator>
-          <Decorator ID="ForceSuccess">
-            <SubTree ID="Close Gripper" />
-          </Decorator>
-          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <SubTree
-              ID="Move to Waypoint"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              joint_group_name="manipulator"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
-              _collapsed="true"
-              waypoint_name="Above Place Cube"
-            />
-          </Decorator>
-          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <SubTree
-              ID="Move to Waypoint"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              joint_group_name="manipulator"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
-              _collapsed="true"
-              waypoint_name="Above Pick Cube"
-            />
-          </Decorator>
-          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Pick Cube"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
-            />
-          </Decorator>
+          <SubTree
+            ID="Move to Waypoint"
+            controller_names="/joint_trajectory_controller"
+            joint_group_name="manipulator"
+            _collapsed="true"
+            waypoint_name="Above Place Cube"
+            velocity_scale_factor="1.0"
+            acceleration_scale_factor="1.0"
+            link_padding="0.01"
+            keep_orientation_tolerance="0.05"
+            keep_orientation="true"
+          />
+          <SubTree
+            ID="Move to Waypoint"
+            controller_names="/joint_trajectory_controller"
+            joint_group_name="manipulator"
+            _collapsed="true"
+            waypoint_name="Place Cube"
+            velocity_scale_factor="1.0"
+            acceleration_scale_factor="1.0"
+            link_padding="0.01"
+            keep_orientation_tolerance="0.05"
+            keep_orientation="true"
+          />
+          <SubTree ID="Close Gripper" />
+          <SubTree
+            ID="Move to Waypoint"
+            controller_names="/joint_trajectory_controller"
+            joint_group_name="manipulator"
+            _collapsed="true"
+            waypoint_name="Above Place Cube"
+            velocity_scale_factor="1.0"
+            acceleration_scale_factor="1.0"
+            link_padding="0.01"
+            keep_orientation_tolerance="0.05"
+            keep_orientation="true"
+          />
+          <SubTree
+            ID="Move to Waypoint"
+            controller_names="/joint_trajectory_controller"
+            joint_group_name="manipulator"
+            _collapsed="true"
+            waypoint_name="Above Pick Cube"
+            velocity_scale_factor="1.0"
+            acceleration_scale_factor="1.0"
+            link_padding="0.01"
+            keep_orientation_tolerance="0.05"
+            keep_orientation="true"
+          />
+          <SubTree
+            ID="Move to Waypoint"
+            waypoint_name="Pick Cube"
+            joint_group_name="manipulator"
+            controller_names="/joint_trajectory_controller"
+            velocity_scale_factor="1.0"
+            acceleration_scale_factor="1.0"
+            link_padding="0.01"
+            keep_orientation_tolerance="0.05"
+            keep_orientation="true"
+          />
           <SubTree ID="Open Gripper" />
         </Control>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Constrained Pick and Place">
-      <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-      </MetadataFields>
-    </SubTree>
+    <SubTree ID="Constrained Pick and Place" />
   </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/cycle_between_waypoints.xml b/src/lab_sim/objectives/cycle_between_waypoints.xml
index e6c79ac7..478c75ad 100644
--- a/src/lab_sim/objectives/cycle_between_waypoints.xml
+++ b/src/lab_sim/objectives/cycle_between_waypoints.xml
@@ -10,17 +10,13 @@
           ID="Move to Waypoint"
           waypoint_name="Above Pick Cube"
           joint_group_name="manipulator"
-          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          planner_interface="pro_rrt"
-          constraints="{constraints}"
+          controller_names="/joint_trajectory_controller"
         />
         <SubTree
           ID="Move to Waypoint"
           waypoint_name="Look at Table"
           joint_group_name="manipulator"
-          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          planner_interface="pro_rrt"
-          constraints="{constraints}"
+          controller_names="/joint_trajectory_controller"
         />
       </Control>
     </Decorator>
diff --git a/src/lab_sim/objectives/move_with_velocity_and_force.xml b/src/lab_sim/objectives/move_with_velocity_and_force.xml
index 32c099e5..2d8040a0 100644
--- a/src/lab_sim/objectives/move_with_velocity_and_force.xml
+++ b/src/lab_sim/objectives/move_with_velocity_and_force.xml
@@ -11,7 +11,6 @@
         _collapsed="true"
         waypoint_name="VFC Start"
         joint_group_name="manipulator"
-        planner_interface="pro_rrt"
         controller_names="/joint_trajectory_controller"
       />
       <Action
diff --git a/src/lab_sim/objectives/my_constraints.yaml b/src/lab_sim/objectives/my_constraints.yaml
deleted file mode 100644
index 36deb8b7..00000000
--- a/src/lab_sim/objectives/my_constraints.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-AppendOrientationConstraint:
-  constraint_frame: "world"
-  link_frame: "grasp_link"
-  orientation:  # radians
-    x: -3.14
-    y: 0.0
-    z: -1.57
-  tolerance:    # radians
-    x: 0.2
-    y: 0.2
-    z: 10.0
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_joint_state.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_joint_state.xml
index a8270920..08e507c2 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_joint_state.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_joint_state.xml
@@ -1,52 +1,48 @@
-<?xml version="1.0" ?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
   <BehaviorTree
     ID="Move to Joint State"
     _description="Move to a specified joint state"
+    _subtreeOnly="true"
   >
-    <Control ID="Fallback" name="root">
-      <Control ID="Sequence">
-        <Action
-          ID="RetrieveJointStateParameter"
-          timeout_sec="-1"
-          joint_state="{target_joint_state}"
-        />
-        <Action
-          ID="InitializeMTCTask"
-          task_id="move_to_joint_state"
-          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          task="{move_to_waypoint_task}"
-        />
-        <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
-        <Action
-          ID="SetupMTCMoveToJointState"
-          joint_state="{target_joint_state}"
-          name="SetupMTCMoveToJointState_First"
-          planning_group_name="manipulator"
-          task="{move_to_waypoint_task}"
-        />
-        <Action
-          ID="PlanMTCTask"
-          solution="{move_to_waypoint_solution}"
-          task="{move_to_waypoint_task}"
-        />
-        <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}" />
-        <Action
-          ID="PublishEmpty"
-          topic="/studio_ui/motion_ended"
-          queue_size="1"
-          use_best_effort="false"
-        />
-      </Control>
-      <Control ID="Sequence">
-        <Action
-          ID="PublishEmpty"
-          topic="/studio_ui/motion_ended"
-          queue_size="1"
-          use_best_effort="false"
-        />
-        <Action ID="AlwaysFailure" />
-      </Control>
+    <Control ID="Sequence">
+      <Action ID="ActivateControllers" controller_names="{controller_names}" />
+      <Action
+        ID="PlanToJointGoal"
+        joint_goal="{target_joint_state}"
+        keep_orientation_tolerance="{keep_orientation_tolerance}"
+        link_padding="{link_padding}"
+        keep_orientation="{keep_orientation}"
+        acceleration_scale_factor="{acceleration_scale_factor}"
+        velocity_scale_factor="{velocity_scale_factor}"
+        planning_group_name="{joint_group_name}"
+      />
+      <Action
+        ID="ExecuteFollowJointTrajectory"
+        execute_follow_joint_trajectory_action_name="{controller_action_server}"
+      />
     </Control>
   </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Move to Joint State">
+      <input_port name="target_joint_state" default="{target_joint_state}" />
+      <input_port name="joint_group_name" default="manipulator" />
+      <input_port name="link_padding" default="0.01" />
+      <input_port name="keep_orientation_tolerance" default="0.05" />
+      <input_port name="keep_orientation" default="false" />
+      <input_port name="velocity_scale_factor" default="1.0" />
+      <input_port name="acceleration_scale_factor" default="1.0" />
+      <input_port
+        name="controller_names"
+        default="joint_trajectory_controller"
+      />
+      <input_port
+        name="controller_action_server"
+        default="/joint_trajectory_controller/follow_joint_trajectory"
+      />
+      <MetadataFields>
+        <Metadata subcategory="Motion - Execute" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml
index 15132ea6..3a8f285c 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml
@@ -1,8 +1,9 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
   <BehaviorTree
     ID="Move to Waypoint"
     _description="This Objective is used when moving to one of the saved waypoints in your site configuration"
+    _subtreeOnly="true"
   >
     <Control ID="Sequence" name="root">
       <Action
@@ -11,33 +12,38 @@
         waypoint_name="{waypoint_name}"
         joint_group_name="{joint_group_name}"
       />
-      <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_named_pose"
+      <SubTree
+        ID="Move to Joint State"
+        _collapsed="true"
+        velocity_scale_factor="{velocity_scale_factor}"
+        acceleration_scale_factor="{acceleration_scale_factor}"
+        link_padding="{link_padding}"
+        keep_orientation_tolerance="{keep_orientation_tolerance}"
+        keep_orientation="{keep_orientation}"
+        target_joint_state="{target_joint_state}"
         controller_names="{controller_names}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
-      <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCMoveToJointState"
-        planning_group_name="{joint_group_name}"
-        planner_interface="{planner_interface}"
-        constraints="{constraints}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_waypoint_solution}"
-        task="{move_to_waypoint_task}"
+        joint_group_name="{joint_group_name}"
+        controller_action_server="{controller_action_server}"
       />
-      <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Waypoint">
-      <input_port name="planner_interface" default="moveit_default" />
+      <input_port name="waypoint_name" default="" />
+      <input_port name="joint_group_name" default="manipulator" />
+      <input_port name="link_padding" default="0.01" />
+      <input_port name="keep_orientation_tolerance" default="0.05" />
+      <input_port name="keep_orientation" default="false" />
+      <input_port name="velocity_scale_factor" default="1.0" />
+      <input_port name="acceleration_scale_factor" default="1.0" />
+      <input_port
+        name="controller_names"
+        default="/joint_trajectory_controller"
+      />
+      <input_port
+        name="controller_action_server"
+        default="/joint_trajectory_controller/follow_joint_trajectory"
+      />
       <MetadataFields>
         <Metadata subcategory="Motion - Execute" />
       </MetadataFields>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/3_waypoint_pick_and_place.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/3_waypoint_pick_and_place.xml
index 40693485..01e0a1e3 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/3_waypoint_pick_and_place.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/3_waypoint_pick_and_place.xml
@@ -13,9 +13,7 @@
         ID="Move to Waypoint"
         waypoint_name="Table"
         joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
-        constraints="{constraints}"
+        controller_names="/joint_trajectory_controller"
       />
       <Action
         ID="MoveGripperAction"
@@ -31,9 +29,7 @@
               ID="Move to Waypoint"
               waypoint_name="Pick"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <Action
               ID="MoveGripperAction"
@@ -44,16 +40,12 @@
               ID="Move to Waypoint"
               waypoint_name="Table"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <SubTree
               ID="Move to Waypoint"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
               waypoint_name="Place"
             />
             <Action
@@ -65,9 +57,7 @@
               ID="Move to Waypoint"
               waypoint_name="Table"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
           </Control>
           <!--Pick object from where it was placed, put it down on the original location, and go back to center pose-->
@@ -76,9 +66,7 @@
               ID="Move to Waypoint"
               waypoint_name="Place"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <Action
               ID="MoveGripperAction"
@@ -89,17 +77,13 @@
               ID="Move to Waypoint"
               waypoint_name="Table"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Pick"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <Action
               ID="MoveGripperAction"
@@ -110,9 +94,7 @@
               ID="Move to Waypoint"
               waypoint_name="Table"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
           </Control>
         </Control>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_landsat.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_landsat.xml
index ce7e2526..0c3f39dd 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_landsat.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_landsat.xml
@@ -14,7 +14,7 @@
         ID="Move to Waypoint"
         waypoint_name="Servo Start"
         joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+        controller_names="/joint_trajectory_controller"
       />
       <SubTree ID="Take Snapshot" _collapsed="false" />
       <SubTree
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml
index f795d79a..c227b0d8 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml
@@ -14,7 +14,7 @@
         ID="Move to Waypoint"
         waypoint_name="View Satellite"
         joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+        controller_names="/joint_trajectory_controller"
       />
       <SubTree ID="Take Snapshot" _collapsed="false" />
       <SubTree
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml
index 758573d9..e93c553d 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml
@@ -14,8 +14,7 @@
         ID="Move to Waypoint"
         waypoint_name="View Satellite"
         joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        use_all_planners="false"
+        controller_names="/joint_trajectory_controller"
       />
       <SubTree ID="Take Snapshot" _collapsed="false" />
       <SubTree
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/cycle_between_waypoints.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/cycle_between_waypoints.xml
index e9bf21b2..6398460f 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/cycle_between_waypoints.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/cycle_between_waypoints.xml
@@ -10,13 +10,13 @@
           ID="Move to Waypoint"
           waypoint_name="Pick"
           joint_group_name="manipulator"
-          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+          controller_names="/joint_trajectory_controller"
         />
         <SubTree
           ID="Move to Waypoint"
           waypoint_name="Place"
           joint_group_name="manipulator"
-          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+          controller_names="/joint_trajectory_controller"
         />
       </Control>
     </Decorator>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/joint_diagnostic.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/joint_diagnostic.xml
index f823ccb7..34b40476 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/joint_diagnostic.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/joint_diagnostic.xml
@@ -11,7 +11,7 @@
         ID="Move to Waypoint"
         waypoint_name="Home"
         joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+        controller_names="/joint_trajectory_controller"
       />
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
@@ -19,13 +19,13 @@
             ID="Move to Waypoint"
             waypoint_name="Wrist 2 Max"
             joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+            controller_names="/joint_trajectory_controller"
           />
           <SubTree
             ID="Move to Waypoint"
             waypoint_name="Wrist 2 Min"
             joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+            controller_names="/joint_trajectory_controller"
           />
         </Control>
       </Decorator>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_joint_state.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_joint_state.xml
index a8270920..08e507c2 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_joint_state.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_joint_state.xml
@@ -1,52 +1,48 @@
-<?xml version="1.0" ?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
   <BehaviorTree
     ID="Move to Joint State"
     _description="Move to a specified joint state"
+    _subtreeOnly="true"
   >
-    <Control ID="Fallback" name="root">
-      <Control ID="Sequence">
-        <Action
-          ID="RetrieveJointStateParameter"
-          timeout_sec="-1"
-          joint_state="{target_joint_state}"
-        />
-        <Action
-          ID="InitializeMTCTask"
-          task_id="move_to_joint_state"
-          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          task="{move_to_waypoint_task}"
-        />
-        <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
-        <Action
-          ID="SetupMTCMoveToJointState"
-          joint_state="{target_joint_state}"
-          name="SetupMTCMoveToJointState_First"
-          planning_group_name="manipulator"
-          task="{move_to_waypoint_task}"
-        />
-        <Action
-          ID="PlanMTCTask"
-          solution="{move_to_waypoint_solution}"
-          task="{move_to_waypoint_task}"
-        />
-        <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}" />
-        <Action
-          ID="PublishEmpty"
-          topic="/studio_ui/motion_ended"
-          queue_size="1"
-          use_best_effort="false"
-        />
-      </Control>
-      <Control ID="Sequence">
-        <Action
-          ID="PublishEmpty"
-          topic="/studio_ui/motion_ended"
-          queue_size="1"
-          use_best_effort="false"
-        />
-        <Action ID="AlwaysFailure" />
-      </Control>
+    <Control ID="Sequence">
+      <Action ID="ActivateControllers" controller_names="{controller_names}" />
+      <Action
+        ID="PlanToJointGoal"
+        joint_goal="{target_joint_state}"
+        keep_orientation_tolerance="{keep_orientation_tolerance}"
+        link_padding="{link_padding}"
+        keep_orientation="{keep_orientation}"
+        acceleration_scale_factor="{acceleration_scale_factor}"
+        velocity_scale_factor="{velocity_scale_factor}"
+        planning_group_name="{joint_group_name}"
+      />
+      <Action
+        ID="ExecuteFollowJointTrajectory"
+        execute_follow_joint_trajectory_action_name="{controller_action_server}"
+      />
     </Control>
   </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Move to Joint State">
+      <input_port name="target_joint_state" default="{target_joint_state}" />
+      <input_port name="joint_group_name" default="manipulator" />
+      <input_port name="link_padding" default="0.01" />
+      <input_port name="keep_orientation_tolerance" default="0.05" />
+      <input_port name="keep_orientation" default="false" />
+      <input_port name="velocity_scale_factor" default="1.0" />
+      <input_port name="acceleration_scale_factor" default="1.0" />
+      <input_port
+        name="controller_names"
+        default="joint_trajectory_controller"
+      />
+      <input_port
+        name="controller_action_server"
+        default="/joint_trajectory_controller/follow_joint_trajectory"
+      />
+      <MetadataFields>
+        <Metadata subcategory="Motion - Execute" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml
index c81b5485..3a8f285c 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
   <BehaviorTree
     ID="Move to Waypoint"
@@ -12,39 +12,38 @@
         waypoint_name="{waypoint_name}"
         joint_group_name="{joint_group_name}"
       />
-      <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_named_pose"
+      <SubTree
+        ID="Move to Joint State"
+        _collapsed="true"
+        velocity_scale_factor="{velocity_scale_factor}"
+        acceleration_scale_factor="{acceleration_scale_factor}"
+        link_padding="{link_padding}"
+        keep_orientation_tolerance="{keep_orientation_tolerance}"
+        keep_orientation="{keep_orientation}"
+        target_joint_state="{target_joint_state}"
         controller_names="{controller_names}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
-      <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCMoveToJointState"
-        planning_group_name="{joint_group_name}"
-        planner_interface="{planner_interface}"
-        constraints="{constraints}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_waypoint_solution}"
-        task="{move_to_waypoint_task}"
+        joint_group_name="{joint_group_name}"
+        controller_action_server="{controller_action_server}"
       />
-      <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Waypoint">
       <input_port name="waypoint_name" default="" />
       <input_port name="joint_group_name" default="manipulator" />
-      <input_port name="planner_interface" default="moveit_default" />
+      <input_port name="link_padding" default="0.01" />
+      <input_port name="keep_orientation_tolerance" default="0.05" />
+      <input_port name="keep_orientation" default="false" />
+      <input_port name="velocity_scale_factor" default="1.0" />
+      <input_port name="acceleration_scale_factor" default="1.0" />
       <input_port
         name="controller_names"
         default="/joint_trajectory_controller"
       />
+      <input_port
+        name="controller_action_server"
+        default="/joint_trajectory_controller/follow_joint_trajectory"
+      />
       <MetadataFields>
         <Metadata subcategory="Motion - Execute" />
       </MetadataFields>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/move_to_joint_state.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/move_to_joint_state.xml
index d46e09bb..08e507c2 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/move_to_joint_state.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/move_to_joint_state.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
   <BehaviorTree
     ID="Move to Joint State"
@@ -6,32 +6,40 @@
     _subtreeOnly="true"
   >
     <Control ID="Sequence">
+      <Action ID="ActivateControllers" controller_names="{controller_names}" />
       <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_joint_state"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        task="{move_to_waypoint_task}"
+        ID="PlanToJointGoal"
+        joint_goal="{target_joint_state}"
+        keep_orientation_tolerance="{keep_orientation_tolerance}"
+        link_padding="{link_padding}"
+        keep_orientation="{keep_orientation}"
+        acceleration_scale_factor="{acceleration_scale_factor}"
+        velocity_scale_factor="{velocity_scale_factor}"
+        planning_group_name="{joint_group_name}"
       />
-      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
       <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCMoveToJointState_First"
-        planning_group_name="manipulator"
-        task="{move_to_waypoint_task}"
-        planner_interface="moveit_default"
+        ID="ExecuteFollowJointTrajectory"
+        execute_follow_joint_trajectory_action_name="{controller_action_server}"
       />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_waypoint_solution}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Joint State">
       <input_port name="target_joint_state" default="{target_joint_state}" />
+      <input_port name="joint_group_name" default="manipulator" />
+      <input_port name="link_padding" default="0.01" />
+      <input_port name="keep_orientation_tolerance" default="0.05" />
+      <input_port name="keep_orientation" default="false" />
+      <input_port name="velocity_scale_factor" default="1.0" />
+      <input_port name="acceleration_scale_factor" default="1.0" />
+      <input_port
+        name="controller_names"
+        default="joint_trajectory_controller"
+      />
+      <input_port
+        name="controller_action_server"
+        default="/joint_trajectory_controller/follow_joint_trajectory"
+      />
       <MetadataFields>
         <Metadata subcategory="Motion - Execute" />
       </MetadataFields>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/move_to_waypoint.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/move_to_waypoint.xml
index c81b5485..3a8f285c 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/move_to_waypoint.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/move_to_waypoint.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
   <BehaviorTree
     ID="Move to Waypoint"
@@ -12,39 +12,38 @@
         waypoint_name="{waypoint_name}"
         joint_group_name="{joint_group_name}"
       />
-      <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_named_pose"
+      <SubTree
+        ID="Move to Joint State"
+        _collapsed="true"
+        velocity_scale_factor="{velocity_scale_factor}"
+        acceleration_scale_factor="{acceleration_scale_factor}"
+        link_padding="{link_padding}"
+        keep_orientation_tolerance="{keep_orientation_tolerance}"
+        keep_orientation="{keep_orientation}"
+        target_joint_state="{target_joint_state}"
         controller_names="{controller_names}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
-      <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCMoveToJointState"
-        planning_group_name="{joint_group_name}"
-        planner_interface="{planner_interface}"
-        constraints="{constraints}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_waypoint_solution}"
-        task="{move_to_waypoint_task}"
+        joint_group_name="{joint_group_name}"
+        controller_action_server="{controller_action_server}"
       />
-      <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Waypoint">
       <input_port name="waypoint_name" default="" />
       <input_port name="joint_group_name" default="manipulator" />
-      <input_port name="planner_interface" default="moveit_default" />
+      <input_port name="link_padding" default="0.01" />
+      <input_port name="keep_orientation_tolerance" default="0.05" />
+      <input_port name="keep_orientation" default="false" />
+      <input_port name="velocity_scale_factor" default="1.0" />
+      <input_port name="acceleration_scale_factor" default="1.0" />
       <input_port
         name="controller_names"
         default="/joint_trajectory_controller"
       />
+      <input_port
+        name="controller_action_server"
+        default="/joint_trajectory_controller/follow_joint_trajectory"
+      />
       <MetadataFields>
         <Metadata subcategory="Motion - Execute" />
       </MetadataFields>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml
index 82d378cb..08e507c2 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml
@@ -1,36 +1,45 @@
-<?xml version="1.0" ?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
   <BehaviorTree
     ID="Move to Joint State"
     _description="Move to a specified joint state"
+    _subtreeOnly="true"
   >
     <Control ID="Sequence">
+      <Action ID="ActivateControllers" controller_names="{controller_names}" />
       <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_joint_state"
-        controller_names="/multi_arm_joint_trajectory_controller"
-        task="{move_to_waypoint_task}"
+        ID="PlanToJointGoal"
+        joint_goal="{target_joint_state}"
+        keep_orientation_tolerance="{keep_orientation_tolerance}"
+        link_padding="{link_padding}"
+        keep_orientation="{keep_orientation}"
+        acceleration_scale_factor="{acceleration_scale_factor}"
+        velocity_scale_factor="{velocity_scale_factor}"
+        planning_group_name="{joint_group_name}"
       />
-      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
       <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCMoveToJointState_First"
-        planning_group_name="multi_arm_manipulator"
-        task="{move_to_waypoint_task}"
-        planner_interface="moveit_default"
+        ID="ExecuteFollowJointTrajectory"
+        execute_follow_joint_trajectory_action_name="{controller_action_server}"
       />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_waypoint_solution}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Joint State">
       <input_port name="target_joint_state" default="{target_joint_state}" />
+      <input_port name="joint_group_name" default="manipulator" />
+      <input_port name="link_padding" default="0.01" />
+      <input_port name="keep_orientation_tolerance" default="0.05" />
+      <input_port name="keep_orientation" default="false" />
+      <input_port name="velocity_scale_factor" default="1.0" />
+      <input_port name="acceleration_scale_factor" default="1.0" />
+      <input_port
+        name="controller_names"
+        default="joint_trajectory_controller"
+      />
+      <input_port
+        name="controller_action_server"
+        default="/joint_trajectory_controller/follow_joint_trajectory"
+      />
       <MetadataFields>
         <Metadata subcategory="Motion - Execute" />
       </MetadataFields>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_named_pose.xml b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_named_pose.xml
deleted file mode 100644
index fedcf7f8..00000000
--- a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_named_pose.xml
+++ /dev/null
@@ -1,38 +0,0 @@
-<?xml version="1.0" ?>
-<root BTCPP_format="4" main_tree_to_execute="Move to Named Pose">
-  <BehaviorTree
-    ID="Move to Named Pose"
-    _description="This Objective is used when moving to one of the saved waypoints in your site configuration"
-  >
-    <Control ID="Sequence" name="root">
-      <Action
-        ID="RetrieveWaypoint"
-        waypoint_joint_state="{target_joint_state}"
-        waypoint_name="{waypoint_name}"
-        joint_group_name="{joint_group_name}"
-      />
-      <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_named_pose"
-        controller_names="/multi_arm_joint_trajectory_controller"
-        task="{move_to_waypoint_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
-      <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCMoveToJointState"
-        planning_group_name="multi_arm_manipulator"
-        planner_interface="{planner_interface}"
-        constraints="{constraints}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_waypoint_solution}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}" />
-    </Control>
-  </BehaviorTree>
-</root>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_waypoint.xml b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_waypoint.xml
new file mode 100644
index 00000000..3a8f285c
--- /dev/null
+++ b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_waypoint.xml
@@ -0,0 +1,52 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
+  <BehaviorTree
+    ID="Move to Waypoint"
+    _description="This Objective is used when moving to one of the saved waypoints in your site configuration"
+    _subtreeOnly="true"
+  >
+    <Control ID="Sequence" name="root">
+      <Action
+        ID="RetrieveWaypoint"
+        waypoint_joint_state="{target_joint_state}"
+        waypoint_name="{waypoint_name}"
+        joint_group_name="{joint_group_name}"
+      />
+      <SubTree
+        ID="Move to Joint State"
+        _collapsed="true"
+        velocity_scale_factor="{velocity_scale_factor}"
+        acceleration_scale_factor="{acceleration_scale_factor}"
+        link_padding="{link_padding}"
+        keep_orientation_tolerance="{keep_orientation_tolerance}"
+        keep_orientation="{keep_orientation}"
+        target_joint_state="{target_joint_state}"
+        controller_names="{controller_names}"
+        joint_group_name="{joint_group_name}"
+        controller_action_server="{controller_action_server}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Move to Waypoint">
+      <input_port name="waypoint_name" default="" />
+      <input_port name="joint_group_name" default="manipulator" />
+      <input_port name="link_padding" default="0.01" />
+      <input_port name="keep_orientation_tolerance" default="0.05" />
+      <input_port name="keep_orientation" default="false" />
+      <input_port name="velocity_scale_factor" default="1.0" />
+      <input_port name="acceleration_scale_factor" default="1.0" />
+      <input_port
+        name="controller_names"
+        default="/joint_trajectory_controller"
+      />
+      <input_port
+        name="controller_action_server"
+        default="/joint_trajectory_controller/follow_joint_trajectory"
+      />
+      <MetadataFields>
+        <Metadata subcategory="Motion - Execute" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_two_arms.xml b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_two_arms.xml
index ce8252d9..bfcf2fe9 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_two_arms.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_two_arms.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Cycle Between Poses">
   <!--//////////-->
   <BehaviorTree
@@ -9,25 +9,38 @@
   >
     <Decorator ID="KeepRunningUntilFailure">
       <Control ID="Sequence">
-        <Decorator ID="RetryUntilSuccessful" num_attempts="-1">
-          <SubTree
-            ID="Move to Named Pose"
-            waypoint_name="Home"
-            joint_group_name="multi_arm_manipulator"
-            planner_interface="moveit_default"
-            _collapsed="false"
-          />
-        </Decorator>
-        <Decorator ID="RetryUntilSuccessful" num_attempts="-1">
-          <SubTree
-            ID="Move to Named Pose"
-            waypoint_name="Flip"
-            joint_group_name="multi_arm_manipulator"
-            planner_interface="moveit_default"
-            _collapsed="false"
-          />
-        </Decorator>
+        <SubTree
+          ID="Move to Waypoint"
+          _collapsed="true"
+          joint_group_name="multi_arm_manipulator"
+          controller_names="/multi_arm_joint_trajectory_controller"
+          velocity_scale_factor="1.0"
+          acceleration_scale_factor="1.0"
+          link_padding="0.01"
+          keep_orientation_tolerance="0.05"
+          keep_orientation="false"
+          waypoint_name="Home"
+          target_joint_state="{target_joint_state}"
+          controller_action_server="/multi_arm_joint_trajectory_controller/follow_joint_trajectory"
+        />
+        <SubTree
+          ID="Move to Waypoint"
+          _collapsed="true"
+          joint_group_name="multi_arm_manipulator"
+          controller_names="/multi_arm_joint_trajectory_controller"
+          velocity_scale_factor="1.0"
+          acceleration_scale_factor="1.0"
+          link_padding="0.01"
+          keep_orientation_tolerance="0.05"
+          keep_orientation="false"
+          waypoint_name="Flip"
+          target_joint_state="{target_joint_state}"
+          controller_action_server="/multi_arm_joint_trajectory_controller/follow_joint_trajectory"
+        />
       </Control>
     </Decorator>
   </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Cycle Between Poses" />
+  </TreeNodesModel>
 </root>