From 3a955261ac78708813ab759b141dd2545c6abc26 Mon Sep 17 00:00:00 2001
From: Michael Wrock <mike.wrock@gmail.com>
Date: Wed, 1 Jan 2025 12:31:53 -0800
Subject: [PATCH 1/2] Updating apriltag objective

---
 .../objectives/apriltag_pick_object.xml       | 112 ++++--------------
 .../pick_april_tag_object_with_approval.xml   |  78 +++++-------
 2 files changed, 51 insertions(+), 139 deletions(-)

diff --git a/src/lab_sim/objectives/apriltag_pick_object.xml b/src/lab_sim/objectives/apriltag_pick_object.xml
index 29dc823a..774c72ac 100644
--- a/src/lab_sim/objectives/apriltag_pick_object.xml
+++ b/src/lab_sim/objectives/apriltag_pick_object.xml
@@ -1,98 +1,34 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
   <!--//////////-->
-  <BehaviorTree
-    ID="Pick April Tag Labeled Object"
-    _description="Picks up an object that has an AprilTag marker."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Pick April Tag Labeled Object" _description="Picks up an object that has an AprilTag marker." _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Look at table" _collapsed="true" />
-      <Action ID="ClearSnapshot" />
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Look at table" _collapsed="true"/>
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
       <Control ID="Sequence" name="Setup">
-        <SubTree ID="Open Gripper" />
+        <SubTree ID="Open Gripper"/>
       </Control>
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="apriltag_detection_config.yaml"
-        parameters="{parameters}"
-      />
-      <Action
-        ID="GetCameraInfo"
-        topic_name="/wrist_camera/camera_info"
-        message_out="{camera_info}"
-      />
-      <Action
-        ID="GetImage"
-        topic_name="/wrist_camera/color"
-        message_out="{image}"
-      />
-      <SubTree
-        ID="Get AprilTag Pose from Image"
-        image="{image}"
-        camera_info="{camera_info}"
-        parameters="{parameters}"
-        _collapsed="true"
-        detection_pose="{detection_pose}"
-      />
-      <Action
-        ID="TransformPose"
-        output_pose="{view_pose}"
-        input_pose="{detection_pose}"
-        quaternion_xyzw="0;0;0;1"
-        translation_xyz="0;-0.1;-0.15"
-      />
-      <SubTree
-        ID="Plan Move To Pose"
-        target_pose="{view_pose}"
-        move_to_pose_solution="{move_to_pose_solution}"
-      />
-      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
-      <SubTree
-        ID="Get AprilTag Pose from Image"
-        image="{image}"
-        camera_info="{camera_info}"
-        parameters="{parameters}"
-        _collapsed="true"
-        detection_pose="{detection_pose}"
-      />
-      <Action
-        ID="TransformPose"
-        output_pose="{offset_pose}"
-        input_pose="{detection_pose}"
-        quaternion_xyzw="0;0;0;1"
-        translation_xyz="0;0;-0.01"
-      />
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="pick_object_config.yaml"
-      />
-      <Action
-        ID="InitializeMTCTask"
-        task_id="pick_object"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        task="{pick_object_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{pick_object_task}" />
-      <Action
-        ID="SetupMTCPickObject"
-        grasp_pose="{offset_pose}"
-        task="{pick_object_task}"
-        parameters="{parameters}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{pick_object_solution}"
-        task="{pick_object_task}"
-      />
-      <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" />
+      <Action ID="LoadObjectiveParameters" config_file_name="apriltag_detection_config.yaml" parameters="{parameters}"/>
+      <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" message_out="{camera_info}" timeout_sec="5.000000"/>
+      <Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}" timeout_sec="5.000000"/>
+      <SubTree ID="Get AprilTag Pose from Image" image="{image}" camera_info="{camera_info}" parameters="{parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
+      <Action ID="TransformPose" output_pose="{view_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;-0.1;-0.15"/>
+      <SubTree ID="Plan Move To Pose" target_pose="{view_pose}" move_to_pose_solution="{move_to_pose_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" goal_duration_tolerance="-1.000000"/>
+      <SubTree ID="Get AprilTag Pose from Image" image="{image}" camera_info="{camera_info}" parameters="{parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
+      <Action ID="TransformPose" output_pose="{offset_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;-0.01"/>
+      <Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml" parameters="{parameters}"/>
+      <Action ID="InitializeMTCTask" task_id="pick_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{pick_object_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{pick_object_task}" skip_collision_check="false"/>
+      <Action ID="SetupMTCPickObject" grasp_pose="{offset_pose}" task="{pick_object_task}" parameters="{parameters}" monitored_stage="current state"/>
+      <Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
+      <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" goal_duration_tolerance="-1.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Pick April Tag Labeled Object">
-      <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-      </MetadataFields>
-    </SubTree>
+    <SubTree ID="Pick April Tag Labeled Object"><MetadataFields>
+        <Metadata subcategory="Application - Advanced Examples"/>
+      </MetadataFields></SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
index 6ca263f1..985130fe 100644
--- a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
+++ b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
@@ -1,60 +1,36 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Pick April Tag Object With Approval"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object With Approval">
   <!--//////////-->
-  <BehaviorTree
-    ID="Pick April Tag Object With Approval"
-    _description="Picks up an object that has an AprilTag marker."
-    _favorite="false"
-  >
+  <BehaviorTree ID="Pick April Tag Labeled Object With Approval" _description="Picks up an object that has an AprilTag marker." _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Look at table" _collapsed="true"/>
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
       <Control ID="Sequence" name="Setup">
-        <SubTree ID="Open Gripper" />
+        <SubTree ID="Open Gripper"/>
       </Control>
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="apriltag_detection_config.yaml"
-        parameters="{parameters}"
-      />
-      <Action
-        ID="GetCameraInfo"
-        topic_name="/wrist_camera/camera_info"
-        message_out="{camera_info}"
-      />
-      <Action
-        ID="GetImage"
-        topic_name="/wrist_camera/color"
-        message_out="{image}"
-      />
-      <Action
-        ID="DetectAprilTags"
-        image="{image}"
-        camera_info="{camera_info}"
-        parameters="{parameters}"
-        detections="{detections}"
-      />
-      <Action ID="GetDetectionPose" />
-      <Action ID="TransformPoseFrame" input_pose="{detection_pose}" />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{initial_poses}"
-        input="{output_pose}"
-      />
-      <SubTree
-        ID="Pick first object in vector"
-        _collapsed="true"
-        pose_vector="{initial_poses}"
-      />
+      <Action ID="LoadObjectiveParameters" config_file_name="apriltag_detection_config.yaml" parameters="{parameters}"/>
+      <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" message_out="{camera_info}" timeout_sec="5.000000"/>
+      <Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}" timeout_sec="5.000000"/>
+      <SubTree ID="Get AprilTag Pose from Image" image="{image}" camera_info="{camera_info}" parameters="{parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
+      <Action ID="TransformPose" output_pose="{view_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;-0.1;-0.15"/>
+      <SubTree ID="Plan Move To Pose" target_pose="{view_pose}" move_to_pose_solution="{move_to_pose_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" goal_duration_tolerance="-1.000000"/>
+      <SubTree ID="Get AprilTag Pose from Image" image="{image}" camera_info="{camera_info}" parameters="{parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
+      <Action ID="TransformPose" output_pose="{offset_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;-0.01"/>
+      <Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml" parameters="{parameters}"/>
+      <Action ID="InitializeMTCTask" task_id="pick_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{pick_object_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{pick_object_task}" skip_collision_check="false"/>
+      <Action ID="SetupMTCPickObject" grasp_pose="{offset_pose}" task="{pick_object_task}" parameters="{parameters}" monitored_stage="current state"/>
+      <Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
+      <Action ID="WaitForUserTrajectoryApproval" solution="{pick_object_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" goal_duration_tolerance="-1.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Pick April Tag Object With Approval">
-      <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-      </MetadataFields>
-    </SubTree>
+    <SubTree ID="Pick April Tag Labeled Object With Approval"><MetadataFields>
+        <Metadata subcategory="Application - Advanced Examples"/>
+        <Metadata runnable="true"/>
+      </MetadataFields></SubTree>
   </TreeNodesModel>
 </root>

From 141fea04067ea726f6643240f7c1247b19ac007f Mon Sep 17 00:00:00 2001
From: Michael Wrock <mike.wrock@gmail.com>
Date: Wed, 1 Jan 2025 12:34:08 -0800
Subject: [PATCH 2/2] updating objective

---
 .../objectives/apriltag_pick_object.xml       | 129 +++++++++++++---
 .../pick_april_tag_object_with_approval.xml   | 141 ++++++++++++++----
 2 files changed, 219 insertions(+), 51 deletions(-)

diff --git a/src/lab_sim/objectives/apriltag_pick_object.xml b/src/lab_sim/objectives/apriltag_pick_object.xml
index 774c72ac..9434c0c8 100644
--- a/src/lab_sim/objectives/apriltag_pick_object.xml
+++ b/src/lab_sim/objectives/apriltag_pick_object.xml
@@ -1,34 +1,115 @@
-<?xml version='1.0' encoding='utf-8'?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
   <!--//////////-->
-  <BehaviorTree ID="Pick April Tag Labeled Object" _description="Picks up an object that has an AprilTag marker." _favorite="true">
+  <BehaviorTree
+    ID="Pick April Tag Labeled Object"
+    _description="Picks up an object that has an AprilTag marker."
+    _favorite="true"
+  >
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Look at table" _collapsed="true"/>
-      <Action ID="ClearSnapshot"/>
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
+      <SubTree ID="Look at table" _collapsed="true" />
+      <Action ID="ClearSnapshot" />
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
       <Control ID="Sequence" name="Setup">
-        <SubTree ID="Open Gripper"/>
+        <SubTree ID="Open Gripper" />
       </Control>
-      <Action ID="LoadObjectiveParameters" config_file_name="apriltag_detection_config.yaml" parameters="{parameters}"/>
-      <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" message_out="{camera_info}" timeout_sec="5.000000"/>
-      <Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}" timeout_sec="5.000000"/>
-      <SubTree ID="Get AprilTag Pose from Image" image="{image}" camera_info="{camera_info}" parameters="{parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
-      <Action ID="TransformPose" output_pose="{view_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;-0.1;-0.15"/>
-      <SubTree ID="Plan Move To Pose" target_pose="{view_pose}" move_to_pose_solution="{move_to_pose_solution}"/>
-      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" goal_duration_tolerance="-1.000000"/>
-      <SubTree ID="Get AprilTag Pose from Image" image="{image}" camera_info="{camera_info}" parameters="{parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
-      <Action ID="TransformPose" output_pose="{offset_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;-0.01"/>
-      <Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml" parameters="{parameters}"/>
-      <Action ID="InitializeMTCTask" task_id="pick_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{pick_object_task}"/>
-      <Action ID="SetupMTCCurrentState" task="{pick_object_task}" skip_collision_check="false"/>
-      <Action ID="SetupMTCPickObject" grasp_pose="{offset_pose}" task="{pick_object_task}" parameters="{parameters}" monitored_stage="current state"/>
-      <Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
-      <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" goal_duration_tolerance="-1.000000"/>
+      <Action
+        ID="LoadObjectiveParameters"
+        config_file_name="apriltag_detection_config.yaml"
+        parameters="{parameters}"
+      />
+      <Action
+        ID="GetCameraInfo"
+        topic_name="/wrist_camera/camera_info"
+        message_out="{camera_info}"
+        timeout_sec="5.000000"
+      />
+      <Action
+        ID="GetImage"
+        topic_name="/wrist_camera/color"
+        message_out="{image}"
+        timeout_sec="5.000000"
+      />
+      <SubTree
+        ID="Get AprilTag Pose from Image"
+        image="{image}"
+        camera_info="{camera_info}"
+        parameters="{parameters}"
+        _collapsed="true"
+        detection_pose="{detection_pose}"
+      />
+      <Action
+        ID="TransformPose"
+        output_pose="{view_pose}"
+        input_pose="{detection_pose}"
+        quaternion_xyzw="0;0;0;1"
+        translation_xyz="0;-0.1;-0.15"
+      />
+      <SubTree
+        ID="Plan Move To Pose"
+        target_pose="{view_pose}"
+        move_to_pose_solution="{move_to_pose_solution}"
+      />
+      <Action
+        ID="ExecuteMTCTask"
+        solution="{move_to_pose_solution}"
+        goal_duration_tolerance="-1.000000"
+      />
+      <SubTree
+        ID="Get AprilTag Pose from Image"
+        image="{image}"
+        camera_info="{camera_info}"
+        parameters="{parameters}"
+        _collapsed="true"
+        detection_pose="{detection_pose}"
+      />
+      <Action
+        ID="TransformPose"
+        output_pose="{offset_pose}"
+        input_pose="{detection_pose}"
+        quaternion_xyzw="0;0;0;1"
+        translation_xyz="0;0;-0.01"
+      />
+      <Action
+        ID="LoadObjectiveParameters"
+        config_file_name="pick_object_config.yaml"
+        parameters="{parameters}"
+      />
+      <Action
+        ID="InitializeMTCTask"
+        task_id="pick_object"
+        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+        task="{pick_object_task}"
+      />
+      <Action
+        ID="SetupMTCCurrentState"
+        task="{pick_object_task}"
+        skip_collision_check="false"
+      />
+      <Action
+        ID="SetupMTCPickObject"
+        grasp_pose="{offset_pose}"
+        task="{pick_object_task}"
+        parameters="{parameters}"
+        monitored_stage="current state"
+      />
+      <Action
+        ID="PlanMTCTask"
+        solution="{pick_object_solution}"
+        task="{pick_object_task}"
+      />
+      <Action
+        ID="ExecuteMTCTask"
+        solution="{pick_object_solution}"
+        goal_duration_tolerance="-1.000000"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Pick April Tag Labeled Object"><MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples"/>
-      </MetadataFields></SubTree>
+    <SubTree ID="Pick April Tag Labeled Object">
+      <MetadataFields>
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+    </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
index 985130fe..51e17012 100644
--- a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
+++ b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
@@ -1,36 +1,123 @@
-<?xml version='1.0' encoding='utf-8'?>
-<root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object With Approval">
+<?xml version="1.0" encoding="utf-8" ?>
+<root
+  BTCPP_format="4"
+  main_tree_to_execute="Pick April Tag Labeled Object With Approval"
+>
   <!--//////////-->
-  <BehaviorTree ID="Pick April Tag Labeled Object With Approval" _description="Picks up an object that has an AprilTag marker." _favorite="false">
+  <BehaviorTree
+    ID="Pick April Tag Labeled Object With Approval"
+    _description="Picks up an object that has an AprilTag marker."
+    _favorite="false"
+  >
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Look at table" _collapsed="true"/>
-      <Action ID="ClearSnapshot"/>
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
+      <SubTree ID="Look at table" _collapsed="true" />
+      <Action ID="ClearSnapshot" />
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
       <Control ID="Sequence" name="Setup">
-        <SubTree ID="Open Gripper"/>
+        <SubTree ID="Open Gripper" />
       </Control>
-      <Action ID="LoadObjectiveParameters" config_file_name="apriltag_detection_config.yaml" parameters="{parameters}"/>
-      <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" message_out="{camera_info}" timeout_sec="5.000000"/>
-      <Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}" timeout_sec="5.000000"/>
-      <SubTree ID="Get AprilTag Pose from Image" image="{image}" camera_info="{camera_info}" parameters="{parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
-      <Action ID="TransformPose" output_pose="{view_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;-0.1;-0.15"/>
-      <SubTree ID="Plan Move To Pose" target_pose="{view_pose}" move_to_pose_solution="{move_to_pose_solution}"/>
-      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" goal_duration_tolerance="-1.000000"/>
-      <SubTree ID="Get AprilTag Pose from Image" image="{image}" camera_info="{camera_info}" parameters="{parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
-      <Action ID="TransformPose" output_pose="{offset_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;-0.01"/>
-      <Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml" parameters="{parameters}"/>
-      <Action ID="InitializeMTCTask" task_id="pick_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{pick_object_task}"/>
-      <Action ID="SetupMTCCurrentState" task="{pick_object_task}" skip_collision_check="false"/>
-      <Action ID="SetupMTCPickObject" grasp_pose="{offset_pose}" task="{pick_object_task}" parameters="{parameters}" monitored_stage="current state"/>
-      <Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
-      <Action ID="WaitForUserTrajectoryApproval" solution="{pick_object_solution}"/>
-      <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" goal_duration_tolerance="-1.000000"/>
+      <Action
+        ID="LoadObjectiveParameters"
+        config_file_name="apriltag_detection_config.yaml"
+        parameters="{parameters}"
+      />
+      <Action
+        ID="GetCameraInfo"
+        topic_name="/wrist_camera/camera_info"
+        message_out="{camera_info}"
+        timeout_sec="5.000000"
+      />
+      <Action
+        ID="GetImage"
+        topic_name="/wrist_camera/color"
+        message_out="{image}"
+        timeout_sec="5.000000"
+      />
+      <SubTree
+        ID="Get AprilTag Pose from Image"
+        image="{image}"
+        camera_info="{camera_info}"
+        parameters="{parameters}"
+        _collapsed="true"
+        detection_pose="{detection_pose}"
+      />
+      <Action
+        ID="TransformPose"
+        output_pose="{view_pose}"
+        input_pose="{detection_pose}"
+        quaternion_xyzw="0;0;0;1"
+        translation_xyz="0;-0.1;-0.15"
+      />
+      <SubTree
+        ID="Plan Move To Pose"
+        target_pose="{view_pose}"
+        move_to_pose_solution="{move_to_pose_solution}"
+      />
+      <Action
+        ID="ExecuteMTCTask"
+        solution="{move_to_pose_solution}"
+        goal_duration_tolerance="-1.000000"
+      />
+      <SubTree
+        ID="Get AprilTag Pose from Image"
+        image="{image}"
+        camera_info="{camera_info}"
+        parameters="{parameters}"
+        _collapsed="true"
+        detection_pose="{detection_pose}"
+      />
+      <Action
+        ID="TransformPose"
+        output_pose="{offset_pose}"
+        input_pose="{detection_pose}"
+        quaternion_xyzw="0;0;0;1"
+        translation_xyz="0;0;-0.01"
+      />
+      <Action
+        ID="LoadObjectiveParameters"
+        config_file_name="pick_object_config.yaml"
+        parameters="{parameters}"
+      />
+      <Action
+        ID="InitializeMTCTask"
+        task_id="pick_object"
+        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+        task="{pick_object_task}"
+      />
+      <Action
+        ID="SetupMTCCurrentState"
+        task="{pick_object_task}"
+        skip_collision_check="false"
+      />
+      <Action
+        ID="SetupMTCPickObject"
+        grasp_pose="{offset_pose}"
+        task="{pick_object_task}"
+        parameters="{parameters}"
+        monitored_stage="current state"
+      />
+      <Action
+        ID="PlanMTCTask"
+        solution="{pick_object_solution}"
+        task="{pick_object_task}"
+      />
+      <Action
+        ID="WaitForUserTrajectoryApproval"
+        solution="{pick_object_solution}"
+      />
+      <Action
+        ID="ExecuteMTCTask"
+        solution="{pick_object_solution}"
+        goal_duration_tolerance="-1.000000"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Pick April Tag Labeled Object With Approval"><MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples"/>
-        <Metadata runnable="true"/>
-      </MetadataFields></SubTree>
+    <SubTree ID="Pick April Tag Labeled Object With Approval">
+      <MetadataFields>
+        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata runnable="true" />
+      </MetadataFields>
+    </SubTree>
   </TreeNodesModel>
 </root>