From 674c57b3cc7dc2a1ce085f1d87d80e19ea26ccff Mon Sep 17 00:00:00 2001
From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
Date: Thu, 1 Feb 2024 14:55:08 -0500
Subject: [PATCH 1/5] Do not use all planners except for constrained planning
 (#198)

---
 .../objectives/3_waypoint_pick_and_place.xml   | 18 +++++++++---------
 .../objectives/cycle_between_waypoints.xml     |  4 ++--
 .../objectives/move_to_joint_state.xml         |  2 +-
 .../objectives/3_waypoint_pick_and_place.xml   | 18 +++++++++---------
 .../objectives/object_inspection.xml           |  2 +-
 .../objectives/3_waypoint_pick_and_place.xml   | 18 +++++++++---------
 .../objectives/cycle_between_waypoints.xml     |  4 ++--
 .../objectives/joint_diagnostic.xml            |  6 +++---
 .../looping_pick_and_place_object.xml          |  8 ++++----
 .../objectives/open_door_fixed_handle_ml.xml   |  6 +++---
 .../objectives/open_door_lever_handle_ml.xml   |  2 +-
 .../objectives/pick_place_object.xml           |  6 +++---
 .../objectives/push_button_ml.xml              |  2 +-
 13 files changed, 48 insertions(+), 48 deletions(-)

diff --git a/src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml
index 83ea1374..84efe835 100644
--- a/src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml
+++ b/src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml
@@ -5,28 +5,28 @@
         <Control ID="Sequence">
             <!-- Clear snapshot, move to center pose, and open gripper -->
             <Action ID="ClearSnapshot" />
-            <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+            <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
             <!-- Keep executing the pick and place sequence until failure -->
             <Decorator ID="KeepRunningUntilFailure">
                 <Control ID="Sequence">
                     <!-- Pick object from "Grab", put it down at "Place", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                     <!-- Pick object from "Place", put it down at "Grab", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                 </Control>
             </Decorator>
diff --git a/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml b/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml
index 7dbaf01c..62c970bf 100644
--- a/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml
+++ b/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml
@@ -3,8 +3,8 @@
     <BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure">
         <Decorator ID="KeepRunningUntilFailure">
             <Control ID="Sequence">
-                <Action ID="MoveToWaypoint" waypoint_name="Right Corner" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                <Action ID="MoveToWaypoint" waypoint_name="Right Corner" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
             </Control>
         </Decorator>
     </BehaviorTree>
diff --git a/src/picknik_ur_base_config/objectives/move_to_joint_state.xml b/src/picknik_ur_base_config/objectives/move_to_joint_state.xml
index d4cf6533..5a0bc540 100644
--- a/src/picknik_ur_base_config/objectives/move_to_joint_state.xml
+++ b/src/picknik_ur_base_config/objectives/move_to_joint_state.xml
@@ -6,7 +6,7 @@
                 <Action ID="RetrieveJointStateParameter" timeout_sec="" 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}" use_all_planners="true" constraints=""/>
+                <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}" use_all_planners="false" constraints=""/>
                 <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"/>
diff --git a/src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml
index d1a2b3c9..9ee8b91c 100644
--- a/src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml
+++ b/src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml
@@ -5,28 +5,28 @@
         <Control ID="Sequence">
             <!-- Clear snapshot, move to center pose, and open gripper -->
             <Action ID="ClearSnapshot" />
-            <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+            <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
             <!-- Keep executing the pick and place sequence until failure -->
             <Decorator ID="KeepRunningUntilFailure">
                 <Control ID="Sequence">
                     <!-- Pick object from "Pick", put it down at "Place", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                     <!-- Pick object from "Place", put it down at "Pick", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                 </Control>
             </Decorator>
diff --git a/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml b/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml
index 4053eaa0..0faa42d1 100644
--- a/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml
+++ b/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml
@@ -10,7 +10,7 @@
       <Decorator ID="ForEachPoseStamped" vector_in="{pose_stamped_msgs}" out="{trajectory_pose}">
         <Control ID="Sequence">
           <Action ID="TransformPoseWithPose" input_pose="{trajectory_pose}" transform_pose="{model_to_real_pose}" output_pose="{target_pose}"/>
-          <Action ID="MoveToPose" target_pose="{target_pose}" ik_frame="grasp_link" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+          <Action ID="MoveToPose" target_pose="{target_pose}" ik_frame="grasp_link" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
           <Action ID="GetSynchronizedCameraTopics" point_cloud_topic_name="/wrist_mounted_camera/depth/color/points" rgb_image_topic_name="/wrist_mounted_camera/color/image_raw" rgb_camera_info_topic_name="/wrist_mounted_camera/color/camera_info" point_cloud="{point_cloud}" rgb_image="{rgb_image}" rgb_camera_info="{rgb_camera_info}"/>
           <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" target_frame="world" output_cloud="{point_cloud_world}"/>
           <Action ID="AddPointCloudToVector" point_cloud="{point_cloud_world}" point_cloud_vector="{point_cloud_vector}"/>
diff --git a/src/picknik_ur_mock_hw_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_ur_mock_hw_config/objectives/3_waypoint_pick_and_place.xml
index ffabac9d..61c2b4ea 100644
--- a/src/picknik_ur_mock_hw_config/objectives/3_waypoint_pick_and_place.xml
+++ b/src/picknik_ur_mock_hw_config/objectives/3_waypoint_pick_and_place.xml
@@ -5,28 +5,28 @@
         <Control ID="Sequence">
             <!-- Clear snapshot, move to center pose, and open gripper -->
             <Action ID="ClearSnapshot" />
-            <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+            <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
             <!-- Keep executing the pick and place sequence until failure -->
             <Decorator ID="KeepRunningUntilFailure">
                 <Control ID="Sequence">
                     <!-- Pick object from left, put it down at the CNC machine, and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Grasp Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Grasp Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                     <!-- Pick object from the CNC machine, put it down on the right, and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Grasp Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Grasp Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                 </Control>
             </Decorator>
diff --git a/src/picknik_ur_mock_hw_config/objectives/cycle_between_waypoints.xml b/src/picknik_ur_mock_hw_config/objectives/cycle_between_waypoints.xml
index 9eb88bfa..ca5b5392 100644
--- a/src/picknik_ur_mock_hw_config/objectives/cycle_between_waypoints.xml
+++ b/src/picknik_ur_mock_hw_config/objectives/cycle_between_waypoints.xml
@@ -3,8 +3,8 @@
     <BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure" _favorite="true">
         <Decorator ID="KeepRunningUntilFailure">
             <Control ID="Sequence">
-                <Action ID="MoveToWaypoint" waypoint_name="Look at Left Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                <Action ID="MoveToWaypoint" waypoint_name="Look at Left Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
             </Control>
         </Decorator>
     </BehaviorTree>
diff --git a/src/picknik_ur_mock_hw_config/objectives/joint_diagnostic.xml b/src/picknik_ur_mock_hw_config/objectives/joint_diagnostic.xml
index de502164..a3c1f3f1 100644
--- a/src/picknik_ur_mock_hw_config/objectives/joint_diagnostic.xml
+++ b/src/picknik_ur_mock_hw_config/objectives/joint_diagnostic.xml
@@ -3,11 +3,11 @@
   <!-- ////////// -->
   <BehaviorTree ID="Joint Diagnostic" _description="Move to a known pose and cycle through the min and max limits of a joint repeatedly" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="MoveToWaypoint" waypoint_name="Home" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+      <Action ID="MoveToWaypoint" waypoint_name="Home" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
-          <Action ID="MoveToWaypoint" waypoint_name="Wrist 2 Max" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-          <Action ID="MoveToWaypoint" waypoint_name="Wrist 2 Min" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+          <Action ID="MoveToWaypoint" waypoint_name="Wrist 2 Max" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+          <Action ID="MoveToWaypoint" waypoint_name="Wrist 2 Min" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
         </Control>
       </Decorator>
     </Control>
diff --git a/src/picknik_ur_site_config/objectives/looping_pick_and_place_object.xml b/src/picknik_ur_site_config/objectives/looping_pick_and_place_object.xml
index e47dba1d..4dff542a 100644
--- a/src/picknik_ur_site_config/objectives/looping_pick_and_place_object.xml
+++ b/src/picknik_ur_site_config/objectives/looping_pick_and_place_object.xml
@@ -10,7 +10,7 @@
             
             <!-- Open the gripper and move to a consistent state. -->
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
-            <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+            <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
             
             <!-- This loops forever because the behavior tree under this decorator never returns FAILURE. -->
             <Decorator ID="KeepRunningUntilFailure">
@@ -71,14 +71,14 @@
                                     </Control>
                                 </Decorator>
                                 <!-- Move home. -->
-                                <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                                <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                                 
                                 <!-- Move to the current place waypoint set by IterateThroughPlaceWaypoints, then open the gripper. -->
-                                <Action ID="MoveToWaypoint" waypoint_name="{place_waypoint}" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                                <Action ID="MoveToWaypoint" waypoint_name="{place_waypoint}" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                                 <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
 
                                 <!-- Finally, return home. -->
-                                <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                                <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                             </Control>
                         </Decorator>
                     </Decorator>
diff --git a/src/picknik_ur_site_config/objectives/open_door_fixed_handle_ml.xml b/src/picknik_ur_site_config/objectives/open_door_fixed_handle_ml.xml
index 22f6b8ba..4ced5725 100644
--- a/src/picknik_ur_site_config/objectives/open_door_fixed_handle_ml.xml
+++ b/src/picknik_ur_site_config/objectives/open_door_fixed_handle_ml.xml
@@ -9,7 +9,7 @@
       <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
       <SubTree ID="Update Admittance Controller" _collapsed="true"/>
       <!-- Look at the desired location where objects (door) will be present -->
-      <Action ID="MoveToWaypoint" waypoint_name="Extended Right" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true" constraints=""/>
+      <Action ID="MoveToWaypoint" waypoint_name="Extended Right" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
       <SubTree ID="Clear Snapshot" _collapsed="true"/>
       <SubTree ID="Reset Planning Scene" _collapsed="true"/>
       <SubTree ID="Detect Door Graspable Object" _collapsed="true" door_object="{door_object}" detection_classes="space booth cabinet door"/>
@@ -18,11 +18,11 @@
       <Control ID="Fallback">
         <SubTree ID="Detect Fixed Handle Graspable Object" _collapsed="true" grasp_pose="{grasp_pose}"/>
         <Control ID="Sequence">
-          <Action ID="MoveToWaypoint" waypoint_name="Extended Right 2" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true" constraints=""/>
+          <Action ID="MoveToWaypoint" waypoint_name="Extended Right 2" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
           <SubTree ID="Detect Fixed Handle Graspable Object" _collapsed="true" grasp_pose="{grasp_pose}"/>
         </Control>
         <Control ID="Sequence">
-          <Action ID="MoveToWaypoint" waypoint_name="Extended Right" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true" constraints=""/>
+          <Action ID="MoveToWaypoint" waypoint_name="Extended Right" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
           <SubTree ID="Detect Fixed Handle Graspable Object" _collapsed="true" grasp_pose="{grasp_pose}"/>
         </Control>
       </Control>
diff --git a/src/picknik_ur_site_config/objectives/open_door_lever_handle_ml.xml b/src/picknik_ur_site_config/objectives/open_door_lever_handle_ml.xml
index 73c7ea37..9c7ad3d0 100644
--- a/src/picknik_ur_site_config/objectives/open_door_lever_handle_ml.xml
+++ b/src/picknik_ur_site_config/objectives/open_door_lever_handle_ml.xml
@@ -9,7 +9,7 @@
       <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
       <SubTree ID="Update Admittance Controller" _collapsed="true"/>
       <!-- Look at the desired location where objects (door) will be present -->
-      <Action ID="MoveToWaypoint" waypoint_name="Extended Right 2" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true" constraints=""/>
+      <Action ID="MoveToWaypoint" waypoint_name="Extended Right 2" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
       <SubTree ID="Clear Snapshot" _collapsed="true"/>
       <SubTree ID="Reset Planning Scene" _collapsed="true"/>
 
diff --git a/src/picknik_ur_site_config/objectives/pick_place_object.xml b/src/picknik_ur_site_config/objectives/pick_place_object.xml
index b711a5b4..052abc7a 100644
--- a/src/picknik_ur_site_config/objectives/pick_place_object.xml
+++ b/src/picknik_ur_site_config/objectives/pick_place_object.xml
@@ -94,15 +94,15 @@
 
                 <!-- The "Then" condition of the TryGraspOuter IfThenElse control. After successful grasp, move to dropoff pose and release object, then return to the Look at Pick and Place Zone pose. -->
                 <Control ID="Sequence">
-                    <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                    <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
-                    <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                    <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                 </Control>
 
                 <!-- The "Else" condition of the TryGraspOuter IfThenElse control. Move to the Look at Pick and Place Zone pose if all retry attempts failed. -->
                 <Control ID="Sequence">
                     <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
-                    <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                    <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     <Action ID="AlwaysFailure"/>
                 </Control>
             </Control>
diff --git a/src/picknik_ur_site_config/objectives/push_button_ml.xml b/src/picknik_ur_site_config/objectives/push_button_ml.xml
index 1900dc7e..b4aab8e3 100644
--- a/src/picknik_ur_site_config/objectives/push_button_ml.xml
+++ b/src/picknik_ur_site_config/objectives/push_button_ml.xml
@@ -4,7 +4,7 @@
     <Control ID="Sequence">
       <Action ID="LoadObjectiveParameters" config_file_name="push_along_axis_config.yaml" parameters="{parameters}"/>
       <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
-      <Action ID="MoveToWaypoint" waypoint_name="Hinge View" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true" constraints=""/>
+      <Action ID="MoveToWaypoint" waypoint_name="Hinge View" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
       <SubTree ID="Clear Snapshot" _collapsed="true"/>
       <SubTree ID="Reset Planning Scene" _collapsed="true"/>
       <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>

From 68cfd07ff7d5c175064a2cb2084f1140163f5d76 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
Date: Tue, 6 Feb 2024 00:57:13 -0500
Subject: [PATCH 2/5] Remove hack to get around double-counted serial package
 installs (#201)

---
 Dockerfile | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index 7b8c1e4f..2ce7de91 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -61,12 +61,6 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
       --from-paths src \
       --ignore-src
 
-# Remove duplicated serial dependency install files, otherwise there will be a build error.
-# NOTE: This can be removed when MoveIt Pro fully separates core and hardware capabilities.
-WORKDIR /opt/overlay_ws
-RUN rm -rf build/serial install/serial
-
-
 ###################################################################
 # Target for the developer build which does not compile any code. #
 ###################################################################

From afd2886fb782cd3be49491fe968240f6abeb27d9 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
Date: Wed, 7 Feb 2024 10:45:44 -0500
Subject: [PATCH 3/5] Add ML tutorial Objectives (#202)

---
 .../objectives/object_segmentation_2d.xml     | 11 ++++++++
 .../objectives/object_segmentation_3d.xml     | 26 +++++++++++++++++++
 .../waypoints/waypoints.yaml                  |  2 +-
 3 files changed, 38 insertions(+), 1 deletion(-)
 create mode 100644 src/picknik_ur_gazebo_config/objectives/object_segmentation_2d.xml
 create mode 100644 src/picknik_ur_gazebo_config/objectives/object_segmentation_3d.xml

diff --git a/src/picknik_ur_gazebo_config/objectives/object_segmentation_2d.xml b/src/picknik_ur_gazebo_config/objectives/object_segmentation_2d.xml
new file mode 100644
index 00000000..df22312e
--- /dev/null
+++ b/src/picknik_ur_gazebo_config/objectives/object_segmentation_2d.xml
@@ -0,0 +1,11 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Object Segmentation 2D">
+  <!-- ////////// -->
+  <BehaviorTree ID="Object Segmentation 2D" _description="Detect objects using a 2D image segmentation model." _favorite="false" _hardcoded="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="MoveToWaypoint" waypoint_name="Extended Right" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
+      <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
+      <Action ID="GetMasks2DAction" image="{image}" action_name="/get_masks_2d_maskrcnn" valid_classes="" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="-1" masks2d="{masks2d}"/>
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/picknik_ur_gazebo_config/objectives/object_segmentation_3d.xml b/src/picknik_ur_gazebo_config/objectives/object_segmentation_3d.xml
new file mode 100644
index 00000000..6ae17d28
--- /dev/null
+++ b/src/picknik_ur_gazebo_config/objectives/object_segmentation_3d.xml
@@ -0,0 +1,26 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Object Segmentation 3D">
+  <!-- ////////// -->
+  <BehaviorTree ID="Object Segmentation 3D" _description="Detect objects using a 2D image segmentation model and convert them to 3D masks and graspable objects." _favorite="false" _hardcoded="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="ClearSnapshot"/>
+      <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
+      <Action ID="MoveToWaypoint" waypoint_name="Extended Right" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
+      <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
+      <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
+      <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
+      <Action ID="GetMasks2DAction" image="{image}" action_name="/get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="0.8" timeout_sec="-1" masks2d="{masks2d}" valid_classes=""/>
+      <Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
+      <Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
+        <Control ID="Sequence">
+          <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
+          <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/>
+        </Control>
+      </Decorator>
+      <Action ID="GetGraspableObjectsFromMasks3D" point_cloud="{point_cloud}" masks3d="{masks3d}" base_frame="world" plane_inlier_threshold="0.01" minimum_face_area="0.000625" face_separation_threshold="0.01" graspable_objects="{graspable_objects}"/>
+      <Decorator ID="ForEachGraspableObject" vector_in="{graspable_objects}" out="{object}">
+        <Action ID="ModifyObjectInPlanningScene" object="{object}" apply_planning_scene_service="/apply_planning_scene"/>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml b/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
index 660a14b8..a5c92532 100644
--- a/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
+++ b/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
@@ -349,7 +349,7 @@
     velocity: []
   name: Monitor View
 - description: ''
-  favorite: true
+  favorite: false
   joint_state:
     effort: []
     header:

From 716efce6c33f1d3489a8617b3248977e5c624ee7 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
Date: Thu, 8 Feb 2024 04:57:06 -0500
Subject: [PATCH 4/5] Start Teleoperate Objective in Waypoint mode (#203)

---
 src/picknik_ur_base_config/objectives/teleoperate.xml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/picknik_ur_base_config/objectives/teleoperate.xml b/src/picknik_ur_base_config/objectives/teleoperate.xml
index 0af0e032..f9b45da4 100644
--- a/src/picknik_ur_base_config/objectives/teleoperate.xml
+++ b/src/picknik_ur_base_config/objectives/teleoperate.xml
@@ -2,6 +2,6 @@
 <root BTCPP_format="4" main_tree_to_execute="Teleoperate">
   <!-- ////////// -->
   <BehaviorTree ID="Teleoperate" _description="Handles the different variations of teleoperation from the web UI. Can be used standalone." _favorite="true" _hardcoded="false">
-    <SubTree ID="Request Teleoperation" enable_user_interaction="false" user_interaction_prompt="" initial_teleop_mode="0"/>
+    <SubTree ID="Request Teleoperation" enable_user_interaction="false" user_interaction_prompt="" initial_teleop_mode="3"/>
   </BehaviorTree>
 </root>

From 4eb3eca2c6fbd28f70f54e4db61d8a96b38e318c Mon Sep 17 00:00:00 2001
From: Mario Prats <mario.prats@picknik.ai>
Date: Thu, 8 Feb 2024 14:07:13 +0100
Subject: [PATCH 5/5] update paths in scan_and_plan objective (#204)

---
 .../objectives/object_inspection.xml                          | 4 ++--
 .../objectives/object_registration.xml                        | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml b/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml
index 0faa42d1..0a2b3251 100644
--- a/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml
+++ b/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml
@@ -5,7 +5,7 @@
     <Control ID="Sequence" name="TopLevelSequence">
       <Action ID="ClearSnapshot"/>
       <Action ID="MoveToWaypoint" waypoint_name="Look at Object" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
-      <SubTree ID="Estimate Object Pose" object_model_file_path="/opt/overlay_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/farnese-hercules-head.stl" guess_position="-0.5;0.0;0.0" guess_orientation="0.0;0.0;0.6442177;0.7648422" icp_max_correspondence_distance="0.5" model_to_real_pose="{model_to_real_pose}"/>
+      <SubTree ID="Estimate Object Pose" object_model_file_path="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/farnese-hercules-head.stl" guess_position="-0.5;0.0;0.0" guess_orientation="0.0;0.0;0.6442177;0.7648422" icp_max_correspondence_distance="0.5" model_to_real_pose="{model_to_real_pose}"/>
       <Action ID="LoadPoseStampedVectorFromYaml" file_path="trajectory_around_hercules.yaml" output="{pose_stamped_msgs}"/>
       <Decorator ID="ForEachPoseStamped" vector_in="{pose_stamped_msgs}" out="{trajectory_pose}">
         <Control ID="Sequence">
@@ -19,7 +19,7 @@
       <Action ID="ClearSnapshot"/>
       <Action ID="MergePointClouds" point_clouds="{point_cloud_vector}" grid_resolution_meters="0.001" merged_cloud="{merged_cloud}"/>
       <Action ID="SendPointCloudToUI" point_cloud="{merged_cloud}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/>
-      <Action ID="SavePointCloudToFile" point_cloud="{merged_cloud}" file_path="~/.config/moveit_studio/saved_behavior_data" file_prefix="pointcloud"/>
+      <Action ID="SavePointCloudToFile" point_cloud="{merged_cloud}" file_path="~/.config/moveit_pro/saved_behavior_data" file_prefix="pointcloud"/>
     </Control>
   </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_registration.xml b/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_registration.xml
index fe9b0818..e2d0503d 100644
--- a/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_registration.xml
+++ b/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_registration.xml
@@ -5,7 +5,7 @@
     <Control ID="Sequence" name="TopLevelSequence">
       <Action ID="ClearSnapshot"/>
       <Action ID="MoveToWaypoint" waypoint_name="Look at Object" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
-      <SubTree ID="Estimate Object Pose" object_model_file_path="/opt/overlay_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/farnese-hercules-head.stl" guess_position="-0.5;0.0;0.0" guess_orientation="0.0;0.0;0.6442177;0.7648422" icp_max_correspondence_distance="0.5" model_to_real_pose="{model_to_real_pose}"/>
+      <SubTree ID="Estimate Object Pose" object_model_file_path="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/farnese-hercules-head.stl" guess_position="-0.5;0.0;0.0" guess_orientation="0.0;0.0;0.6442177;0.7648422" icp_max_correspondence_distance="0.5" model_to_real_pose="{model_to_real_pose}"/>
     </Control>
   </BehaviorTree>
 </root>