From 0a895552d39008d5a14de387d73471c475f8833d Mon Sep 17 00:00:00 2001
From: Noah Wardlow <noahwardlow@yahoo.com>
Date: Mon, 22 May 2023 13:19:05 -0600
Subject: [PATCH 1/3] Create move_to_pose.xml

---
 .../objectives/move_to_pose.xml                | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)
 create mode 100644 src/picknik_ur_base_config/objectives/move_to_pose.xml

diff --git a/src/picknik_ur_base_config/objectives/move_to_pose.xml b/src/picknik_ur_base_config/objectives/move_to_pose.xml
new file mode 100644
index 00000000..d45f1c1d
--- /dev/null
+++ b/src/picknik_ur_base_config/objectives/move_to_pose.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Move to Pose">
+    <BehaviorTree ID="Move to Pose" _description="Uses inverse kinematics to move the robot to a set gripper position">
+        <Sequence>
+            <Action ID="InitializeMTCTask" task_id="inspect_surface" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
+            <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
+            <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{inspect_surface.target_pose}" task="{move_to_pose_task}"/>
+            <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+            <Fallback name="wait_for_approval_if_user_available">
+                <Inverter>
+                    <Action ID="IsUserAvailable" />
+                </Inverter>
+                <Action ID="WaitForUserTrajectoryApproval" solution="{move_to_pose_solution}" />
+            </Fallback>
+            <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
+        </Sequence>
+    </BehaviorTree>
+</root>

From 44a05d5e2bf062a2f1a37a8fc18c98dd96d2f44a Mon Sep 17 00:00:00 2001
From: Noah Wardlow <108752030+noah-wardlow@users.noreply.github.com>
Date: Mon, 22 May 2023 14:54:16 -0600
Subject: [PATCH 2/3] Update move_to_pose.xml

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

diff --git a/src/picknik_ur_base_config/objectives/move_to_pose.xml b/src/picknik_ur_base_config/objectives/move_to_pose.xml
index d45f1c1d..861dd29c 100644
--- a/src/picknik_ur_base_config/objectives/move_to_pose.xml
+++ b/src/picknik_ur_base_config/objectives/move_to_pose.xml
@@ -2,7 +2,7 @@
 <root BTCPP_format="4" main_tree_to_execute="Move to Pose">
     <BehaviorTree ID="Move to Pose" _description="Uses inverse kinematics to move the robot to a set gripper position">
         <Sequence>
-            <Action ID="InitializeMTCTask" task_id="inspect_surface" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
+            <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
             <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
             <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{inspect_surface.target_pose}" task="{move_to_pose_task}"/>
             <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>

From 70694907c92e8ee055c307c249fdee924ee35a4d Mon Sep 17 00:00:00 2001
From: Noah Wardlow <108752030+noah-wardlow@users.noreply.github.com>
Date: Mon, 22 May 2023 14:57:56 -0600
Subject: [PATCH 3/3] Update move_to_pose.xml

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

diff --git a/src/picknik_ur_base_config/objectives/move_to_pose.xml b/src/picknik_ur_base_config/objectives/move_to_pose.xml
index 861dd29c..c9aeda4d 100644
--- a/src/picknik_ur_base_config/objectives/move_to_pose.xml
+++ b/src/picknik_ur_base_config/objectives/move_to_pose.xml
@@ -4,7 +4,7 @@
         <Sequence>
             <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
             <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
-            <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{inspect_surface.target_pose}" task="{move_to_pose_task}"/>
+            <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{move_to_pose.target_pose}" task="{move_to_pose_task}"/>
             <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
             <Fallback name="wait_for_approval_if_user_available">
                 <Inverter>