From 88d28b35d44fd214ea15062873815598af9ec876 Mon Sep 17 00:00:00 2001
From: Abishalini <abi.gpuram@gmail.com>
Date: Thu, 1 Jun 2023 16:48:28 -0600
Subject: [PATCH 1/7] Modify objectives to use Get String/Pose/JointState From
 User behavior

---
 .../objectives/close_cabinet_door.xml                       | 3 ++-
 src/picknik_ur_base_config/objectives/inspect_surface.xml   | 3 ++-
 .../objectives/interpolate_to_joint_state.xml               | 3 ++-
 .../objectives/interpolate_to_named_state.xml               | 3 ++-
 .../objectives/move_to_joint_state.xml                      | 5 ++++-
 .../objectives/move_to_known_pose.xml                       | 4 +++-
 src/picknik_ur_base_config/objectives/move_to_pose.xml      | 3 ++-
 .../objectives/open_lever_handle_door.xml                   | 6 ++++--
 src/picknik_ur_base_config/objectives/pick_object.xml       | 3 ++-
 src/picknik_ur_base_config/objectives/pick_place_object.xml | 6 ++++--
 src/picknik_ur_base_config/objectives/push_button.xml       | 3 ++-
 src/picknik_ur_base_config/objectives/take_snapshot.xml     | 3 ++-
 .../objectives/open_door_affordance.xml                     | 5 ++++-
 13 files changed, 35 insertions(+), 15 deletions(-)

diff --git a/src/picknik_ur_base_config/objectives/close_cabinet_door.xml b/src/picknik_ur_base_config/objectives/close_cabinet_door.xml
index 4fe22a32..55235ba7 100644
--- a/src/picknik_ur_base_config/objectives/close_cabinet_door.xml
+++ b/src/picknik_ur_base_config/objectives/close_cabinet_door.xml
@@ -8,7 +8,8 @@
             <Control ID="Sequence">
                 <Action ID="InitializeMTCTask" task_id="close_cabinet_door" controller_names="/joint_trajectory_controller" task="{close_cabinet_door_task}"/>
                 <Action ID="SetupMTCCurrentState" task="{close_cabinet_door_task}"/>
-                <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{close_cabinet_door.target_pose}" task="{close_cabinet_door_task}"/>
+                <Action ID="GetPoseFromUser" parameter_name="close_cabinet_door.target_pose" parameter_value="{target_pose}" />
+                <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
                 <Action ID="SetupMTCMoveAlongFrameAxis" axis="z" max_distance="1.5" min_distance="0.05" parameters="{parameters}" task="{close_cabinet_door_task}" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
diff --git a/src/picknik_ur_base_config/objectives/inspect_surface.xml b/src/picknik_ur_base_config/objectives/inspect_surface.xml
index 09274184..72feaa7f 100644
--- a/src/picknik_ur_base_config/objectives/inspect_surface.xml
+++ b/src/picknik_ur_base_config/objectives/inspect_surface.xml
@@ -4,7 +4,8 @@
         <Control ID="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="GetPoseFromUser" parameter_name="inspect_surface.target_pose" parameter_value="{target_pose}" />
+            <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_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>
diff --git a/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml b/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
index d25f4a13..e9d47036 100644
--- a/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
+++ b/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
@@ -2,7 +2,8 @@
 <root BTCPP_format="4" main_tree_to_execute="Interpolate to Joint State">
     <BehaviorTree ID="Interpolate to Joint State" _description="Move to a specified joint state">
         <Control ID="Sequence" name="root">
-            <SubTree ID="InterpolateToJointStateWaypoint" target_joint_state="{interpolate_to_joint_state.target_joint_state}"/>
+            <Action ID="GetJointStateFromUser" parameter_name="interpolate_to_joint_state.target_joint_state" parameter_value="{target_joint_state}" />
+            <SubTree ID="InterpolateToJointStateWaypoint" target_joint_state="{target_joint_state}"/>
         </Control>
     </BehaviorTree>
     <BehaviorTree ID="InterpolateToJointStateWaypoint">
diff --git a/src/picknik_ur_base_config/objectives/interpolate_to_named_state.xml b/src/picknik_ur_base_config/objectives/interpolate_to_named_state.xml
index b117d47b..7b0cff9f 100644
--- a/src/picknik_ur_base_config/objectives/interpolate_to_named_state.xml
+++ b/src/picknik_ur_base_config/objectives/interpolate_to_named_state.xml
@@ -2,7 +2,8 @@
 <root BTCPP_format="4" main_tree_to_execute="Interpolate to Named State">
     <BehaviorTree ID="Interpolate to Named State" _description="Move to a specified named waypoint">
         <Control ID="Sequence" name="root">
-            <SubTree ID="InterpolateToNamedStateWaypoint" target_named_state="{interpolate_to_named_state.target_named_state}"/>
+            <Action ID="GetStringFromUser" parameter_name="interpolate_to_named_state.target_named_state" parameter_value="{target_named_state}" />
+            <SubTree ID="InterpolateToNamedStateWaypoint" target_named_state="{target_named_state}"/>
         </Control>
     </BehaviorTree>
     <BehaviorTree ID="InterpolateToNamedStateWaypoint">
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 105ab516..445f526d 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
@@ -1,6 +1,9 @@
 <?xml version="1.0"?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
     <BehaviorTree ID="Move to Joint State" _description="Single-node behavior to move to a specified joint state">
-        <Action ID="MoveToJointState" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="{move_to_joint_state.waypoint_name}" />
+        <Control ID="Sequence">
+            <Action ID="GetStringFromUser" parameter_name="move_to_joint_state.waypoint_name" parameter_value="{waypoint_name}" />
+            <Action ID="MoveToJointState" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="{waypoint_name}" />
+        </Control>
     </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_base_config/objectives/move_to_known_pose.xml b/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
index 53161707..dba0fc0f 100644
--- a/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
+++ b/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
@@ -2,7 +2,9 @@
 <root BTCPP_format="4" main_tree_to_execute="Move to Known Pose">
     <BehaviorTree ID="Move to Known Pose" _description="Move to a known pose">
         <Control ID="Sequence" name="root">
-            <SubTree ID="MoveToWaypoint" waypoint_name="{move_to_known_pose.target_state_name}"/>
+            <!-- When the user clicks on different pose buttons on Endpoint tab, the frontend sets move_to_known_pose.target_state_name parameter value-->
+            <Action ID="GetStringFromUser" parameter_name="move_to_known_pose.target_state_name" parameter_value="{target_state_name}"/>
+            <SubTree ID="MoveToWaypoint" waypoint_name="{target_state_name}"/>
         </Control>
     </BehaviorTree>
     <BehaviorTree ID="MoveToWaypoint">
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 a16a58c4..220cbe5a 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,8 @@
         <Control ID="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="{move_to_pose.target_pose}" task="{move_to_pose_task}"/>
+            <Action ID="GetPoseFromUser" parameter_name="move_to_pose.target_pose" parameter_value="{target_pose}" />
+            <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_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>
diff --git a/src/picknik_ur_base_config/objectives/open_lever_handle_door.xml b/src/picknik_ur_base_config/objectives/open_lever_handle_door.xml
index 3416ee5c..7bdeff63 100644
--- a/src/picknik_ur_base_config/objectives/open_lever_handle_door.xml
+++ b/src/picknik_ur_base_config/objectives/open_lever_handle_door.xml
@@ -8,8 +8,10 @@
             <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" />
             <Action ID="PublishPointCloud" point_cloud="{point_cloud}"
                 point_cloud_topic="studio_vision_debug_snapshot" />
-            <Action ID="GetDoorHandle" handle_pivot_pose="{get_door_handle_pose.handle_pivot_pose}"
-                handle_tip_pose="{get_door_handle_pose.handle_tip_pose}" parameters="{parameters}"
+            <Action ID="GetPoseFromUser" parameter_name="get_door_handle_pose.handle_pivot_pose" parameter_value="{handle_pivot_pose}" />
+            <Action ID="GetPoseFromUser" parameter_name="get_door_handle_pose.handle_tip_pose" parameter_value="{handle_tip_pose}" />
+            <Action ID="GetDoorHandle" handle_pivot_pose="{handle_pivot_pose}"
+                handle_tip_pose="{handle_tip_pose}" parameters="{parameters}"
                 point_cloud="{point_cloud}" target_handle_length="{handle_length}"
                 target_handle_pose="{handle_pose}" target_handle_z_offset="{handle_z_offset}" />
             <SubTree ID="CloseGripper" />
diff --git a/src/picknik_ur_base_config/objectives/pick_object.xml b/src/picknik_ur_base_config/objectives/pick_object.xml
index 0e658bc6..73bed009 100644
--- a/src/picknik_ur_base_config/objectives/pick_object.xml
+++ b/src/picknik_ur_base_config/objectives/pick_object.xml
@@ -3,7 +3,8 @@
     <BehaviorTree ID="Pick Object" _description="Pick up and lift a small object">
         <Control ID="Sequence" name="root">
             <SubTree ID="OpenGripper"/>
-            <SubTree ID="PickObject" grasp_pose="{pick_object.grasp_pose}"/>
+            <Action ID="GetPoseFromUser" parameter_name="pick_object.grasp_pose" parameter_value="{grasp_pose}" />
+            <SubTree ID="PickObject" grasp_pose="{grasp_pose}"/>
         </Control>
     </BehaviorTree>
     <BehaviorTree ID="PickObject">
diff --git a/src/picknik_ur_base_config/objectives/pick_place_object.xml b/src/picknik_ur_base_config/objectives/pick_place_object.xml
index 630744d1..7724ee8e 100644
--- a/src/picknik_ur_base_config/objectives/pick_place_object.xml
+++ b/src/picknik_ur_base_config/objectives/pick_place_object.xml
@@ -24,7 +24,8 @@
                             <!-- Detect all cuboids in that point cloud-->
                             <Action ID="FindSingularCuboids" point_cloud="{point_cloud}" parameters="{parameters}" detected_shapes="{cuboids}"/>
                             <!-- Find the closest cuboid to the pose calculated from the user's original click -->
-                            <Action ID="GetClosestObjectToPose" objects="{cuboids}" pose="{pick_place_object.grasp_pose}" distance_threshold="0.3" closest_object="{cuboid_object}"/>
+                            <Action ID="GetPoseFromUser" parameter_name="pick_place_object.grasp_pose" parameter_value="{grasp_pose}" />
+                            <Action ID="GetClosestObjectToPose" objects="{cuboids}" pose="{grasp_pose}" distance_threshold="0.3" closest_object="{cuboid_object}"/>
                             <!-- Add this cuboid to the planning scene. -->
                             <Action ID="ModifyObjectInPlanningScene" object="{cuboid_object}" apply_planning_scene_service="/apply_planning_scene"/>
                              <!-- Use point cloud to update the collision octomap (happens after ModifyObjectInPlanningScene so the cuboid is excluded from the octomap)-->
@@ -67,7 +68,8 @@
                                         <Control ID="Sequence">
                                             <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" />
                                             <Action ID="FindSingularCuboids" parameters="{parameters}" point_cloud="{point_cloud}" detected_shapes="{cuboids}"/>
-                                            <Action ID="GetClosestObjectToPose" objects="{cuboids}" pose="{pick_place_object.grasp_pose}" distance_threshold="0.3" closest_object="{new_cuboid_object}"/>
+                                            <Action ID="GetPoseFromUser" parameter_name="pick_place_object.grasp_pose" parameter_value="{grasp_pose}" />
+                                            <Action ID="GetClosestObjectToPose" objects="{cuboids}" pose="{grasp_pose}" distance_threshold="0.3" closest_object="{new_cuboid_object}"/>
                                             <Action ID="CheckCuboidSimilarity" input_cuboid="{new_cuboid_object}" reference_cuboid="{cuboid_object}" base_frame="world" distance_threshold="0.04" orientation_threshold="3.14" />
                                         </Control>
 
diff --git a/src/picknik_ur_base_config/objectives/push_button.xml b/src/picknik_ur_base_config/objectives/push_button.xml
index e655261e..7c0dcd75 100644
--- a/src/picknik_ur_base_config/objectives/push_button.xml
+++ b/src/picknik_ur_base_config/objectives/push_button.xml
@@ -8,7 +8,8 @@
             <Control ID="Sequence">
                 <Action ID="InitializeMTCTask" task_id="push_button" controller_names="/joint_trajectory_controller" task="{push_button_task}"/>
                 <Action ID="SetupMTCCurrentState" task="{push_button_task}"/>
-                <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{push_button.target_pose}" task="{push_button_task}"/>
+                <Action ID="GetPoseFromUser" parameter_name="push_button.target_pose" parameter_value="{target_pose}" />
+                <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{push_button_task}"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
                 <Action ID="SetupMTCMoveAlongFrameAxis" axis="z" max_distance="0.2" min_distance="0.05" parameters="{parameters}" task="{push_button_task}" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
diff --git a/src/picknik_ur_base_config/objectives/take_snapshot.xml b/src/picknik_ur_base_config/objectives/take_snapshot.xml
index 293455a6..b3c3b792 100644
--- a/src/picknik_ur_base_config/objectives/take_snapshot.xml
+++ b/src/picknik_ur_base_config/objectives/take_snapshot.xml
@@ -5,7 +5,8 @@
         <Control ID="Sequence">
             <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" />
             <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
-            <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures" point_cloud_uuid="{take_snapshot.uuid}"/>
+            <Action ID="GetStringFromUser" parameter_name="take_snapshot.uuid" parameter_value="{uuid}" />  
+            <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures" point_cloud_uuid="{uuid}"/>
         </Control>
     </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_gazebo_config/objectives/open_door_affordance.xml b/src/picknik_ur_gazebo_config/objectives/open_door_affordance.xml
index 0925d927..5828dbb0 100644
--- a/src/picknik_ur_gazebo_config/objectives/open_door_affordance.xml
+++ b/src/picknik_ur_gazebo_config/objectives/open_door_affordance.xml
@@ -3,7 +3,10 @@
     <BehaviorTree ID="Open Cabinet Door" _description="Open a cabinet door by grasping and pulling its handle">
         <Control ID="Sequence" name="Setup">
             <Action ID="LoadObjectiveParameters" config_file_name="open_door_affordance_config.yaml" parameters="{parameters}" />
-            <Action ID="GetHingeAxisFromSurfaceSelection" grasp_pose="{grasp_pose}" hinge_axis_pose_end="{process_door_selection.hinge_axis_pose_end}" hinge_axis_pose_start="{process_door_selection.hinge_axis_pose_start}" screw_axis_pose="{screw_axis_pose}" screw_origin_pose="{screw_origin_pose}" target_grasp_pose="{process_door_selection.grasp_pose}" />
+            <Action ID="GetPoseFromUser" parameter_name="process_door_selection.hinge_axis_pose_end" parameter_value="{hinge_axis_pose_end}" />
+            <Action ID="GetPoseFromUser" parameter_name="process_door_selection.hinge_axis_pose_start" parameter_value="{hinge_axis_pose_start}" />
+            <Action ID="GetPoseFromUser" parameter_name="process_door_selection.grasp_pose" parameter_value="{grasp_pose}" />
+            <Action ID="GetHingeAxisFromSurfaceSelection" grasp_pose="{grasp_pose}" hinge_axis_pose_end="{hinge_axis_pose_end}" hinge_axis_pose_start="{hinge_axis_pose_start}" screw_axis_pose="{screw_axis_pose}" screw_origin_pose="{screw_origin_pose}" target_grasp_pose="{grasp_pose}" />
             <Control ID="Sequence" name="OpenDoorAffordanceMain">
                 <Action ID="InitializeMTCTask" task_id="open_door_affordance" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{open_door_affordance_task}" />
                 <Action ID="SetupMTCCurrentState" task="{open_door_affordance_task}" />

From 1570a0b5c9a2cfe12ac88a27246d4541d1292aaf Mon Sep 17 00:00:00 2001
From: Abishalini <abi.gpuram@gmail.com>
Date: Tue, 6 Jun 2023 07:58:44 -0600
Subject: [PATCH 2/7] Use Fallback node to add error message

---
 .../objectives/interpolate_to_joint_state.xml                 | 3 +++
 src/picknik_ur_base_config/objectives/move_to_known_pose.xml  | 4 ++++
 2 files changed, 7 insertions(+)

diff --git a/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml b/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
index e9d47036..dbf78973 100644
--- a/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
+++ b/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
@@ -2,7 +2,10 @@
 <root BTCPP_format="4" main_tree_to_execute="Interpolate to Joint State">
     <BehaviorTree ID="Interpolate to Joint State" _description="Move to a specified joint state">
         <Control ID="Sequence" name="root">
+            <Fallback name="wait_for_approval_if_user_available">
             <Action ID="GetJointStateFromUser" parameter_name="interpolate_to_joint_state.target_joint_state" parameter_value="{target_joint_state}" />
+                <Action ID="LogMessage" message="This objective is used by joint sliders in the Joints tab." log_level="error" />
+            </Fallback>
             <SubTree ID="InterpolateToJointStateWaypoint" target_joint_state="{target_joint_state}"/>
         </Control>
     </BehaviorTree>
diff --git a/src/picknik_ur_base_config/objectives/move_to_known_pose.xml b/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
index dba0fc0f..a229ddbe 100644
--- a/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
+++ b/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
@@ -3,6 +3,10 @@
     <BehaviorTree ID="Move to Known Pose" _description="Move to a known pose">
         <Control ID="Sequence" name="root">
             <!-- When the user clicks on different pose buttons on Endpoint tab, the frontend sets move_to_known_pose.target_state_name parameter value-->
+            <Fallback name="log_error_if_parameter_not_set">
+                <Action ID="GetStringFromUser" parameter_name="move_to_known_pose.target_state_name" parameter_value="{target_state_name}"/>
+                <Action ID="LogMessage" message="This objective is used by Move To Pose buttons in the Endpoint tab." log_level="error" />
+            </Fallback>
             <Action ID="GetStringFromUser" parameter_name="move_to_known_pose.target_state_name" parameter_value="{target_state_name}"/>
             <SubTree ID="MoveToWaypoint" waypoint_name="{target_state_name}"/>
         </Control>

From 24b8e685be56e79e6f30b69b2389db989781beef Mon Sep 17 00:00:00 2001
From: Abishalini <abi.gpuram@gmail.com>
Date: Tue, 6 Jun 2023 10:35:07 -0600
Subject: [PATCH 3/7] Delete interpolate_to_named_state objective

---
 .../objectives/interpolate_to_named_state.xml | 19 -------------------
 1 file changed, 19 deletions(-)
 delete mode 100644 src/picknik_ur_base_config/objectives/interpolate_to_named_state.xml

diff --git a/src/picknik_ur_base_config/objectives/interpolate_to_named_state.xml b/src/picknik_ur_base_config/objectives/interpolate_to_named_state.xml
deleted file mode 100644
index 7b0cff9f..00000000
--- a/src/picknik_ur_base_config/objectives/interpolate_to_named_state.xml
+++ /dev/null
@@ -1,19 +0,0 @@
-<?xml version="1.0"?>
-<root BTCPP_format="4" main_tree_to_execute="Interpolate to Named State">
-    <BehaviorTree ID="Interpolate to Named State" _description="Move to a specified named waypoint">
-        <Control ID="Sequence" name="root">
-            <Action ID="GetStringFromUser" parameter_name="interpolate_to_named_state.target_named_state" parameter_value="{target_named_state}" />
-            <SubTree ID="InterpolateToNamedStateWaypoint" target_named_state="{target_named_state}"/>
-        </Control>
-    </BehaviorTree>
-    <BehaviorTree ID="InterpolateToNamedStateWaypoint">
-        <Control ID="Sequence" name="interpolate_to_waypoint_main">
-            <Action ID="RetrieveWaypoint" waypoint_joint_state="{joint_state}" waypoint_name="{target_named_state}"/>
-            <Action ID="InitializeMTCTask" task_id="interpolate_to_named_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{interpolate_to_waypoint_task}"/>
-            <Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}"/>
-            <Action ID="SetupMTCInterpolateToJointState" joint_state="{joint_state}" name="SetupMTCInterpolateToJointState_First" planning_group_name="manipulator" task="{interpolate_to_waypoint_task}"/>
-            <Action ID="PlanMTCTask" solution="{interpolate_to_waypoint_solution}" task="{interpolate_to_waypoint_task}"/>
-            <Action ID="ExecuteMTCTask" solution="{interpolate_to_waypoint_solution}"/>
-        </Control>
-    </BehaviorTree>
-</root>

From 59e478d3e30c4f884e9e6861de151a9013a36fb3 Mon Sep 17 00:00:00 2001
From: Abishalini <abi.gpuram@gmail.com>
Date: Tue, 6 Jun 2023 15:46:14 -0600
Subject: [PATCH 4/7] fix indentation and use Control node

---
 .../objectives/interpolate_to_joint_state.xml               | 6 +++---
 .../objectives/move_to_known_pose.xml                       | 5 ++---
 2 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml b/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
index dbf78973..3b7c6401 100644
--- a/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
+++ b/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
@@ -2,10 +2,10 @@
 <root BTCPP_format="4" main_tree_to_execute="Interpolate to Joint State">
     <BehaviorTree ID="Interpolate to Joint State" _description="Move to a specified joint state">
         <Control ID="Sequence" name="root">
-            <Fallback name="wait_for_approval_if_user_available">
-            <Action ID="GetJointStateFromUser" parameter_name="interpolate_to_joint_state.target_joint_state" parameter_value="{target_joint_state}" />
+            <Control ID="Fallback" name="log_error_if_parameter_not_set">
+                <Action ID="GetJointStateFromUser" parameter_name="interpolate_to_joint_state.target_joint_state" parameter_value="{target_joint_state}" />
                 <Action ID="LogMessage" message="This objective is used by joint sliders in the Joints tab." log_level="error" />
-            </Fallback>
+            </Control>
             <SubTree ID="InterpolateToJointStateWaypoint" target_joint_state="{target_joint_state}"/>
         </Control>
     </BehaviorTree>
diff --git a/src/picknik_ur_base_config/objectives/move_to_known_pose.xml b/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
index a229ddbe..532c135c 100644
--- a/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
+++ b/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
@@ -3,11 +3,10 @@
     <BehaviorTree ID="Move to Known Pose" _description="Move to a known pose">
         <Control ID="Sequence" name="root">
             <!-- When the user clicks on different pose buttons on Endpoint tab, the frontend sets move_to_known_pose.target_state_name parameter value-->
-            <Fallback name="log_error_if_parameter_not_set">
+            <Control ID="Fallback" name="log_error_if_parameter_not_set">
                 <Action ID="GetStringFromUser" parameter_name="move_to_known_pose.target_state_name" parameter_value="{target_state_name}"/>
                 <Action ID="LogMessage" message="This objective is used by Move To Pose buttons in the Endpoint tab." log_level="error" />
-            </Fallback>
-            <Action ID="GetStringFromUser" parameter_name="move_to_known_pose.target_state_name" parameter_value="{target_state_name}"/>
+            </Control>
             <SubTree ID="MoveToWaypoint" waypoint_name="{target_state_name}"/>
         </Control>
     </BehaviorTree>

From c3d0ce4ed494bbb0154d4da7cb67a72631cbad3c Mon Sep 17 00:00:00 2001
From: Abishalini <abi.gpuram@gmail.com>
Date: Tue, 6 Jun 2023 16:31:34 -0600
Subject: [PATCH 5/7] Add AlwaysFailure node to end objective after logging

---
 .../objectives/interpolate_to_joint_state.xml                | 5 ++++-
 src/picknik_ur_base_config/objectives/move_to_known_pose.xml | 5 ++++-
 2 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml b/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
index 3b7c6401..cb0c2828 100644
--- a/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
+++ b/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
@@ -4,7 +4,10 @@
         <Control ID="Sequence" name="root">
             <Control ID="Fallback" name="log_error_if_parameter_not_set">
                 <Action ID="GetJointStateFromUser" parameter_name="interpolate_to_joint_state.target_joint_state" parameter_value="{target_joint_state}" />
-                <Action ID="LogMessage" message="This objective is used by joint sliders in the Joints tab." log_level="error" />
+                <Control ID="Sequence">
+                    <Action ID="LogMessage" message="This objective is meant to be used by joint sliders in the Joints tab. Try it out there!" log_level="error" />
+                    <Action ID="AlwaysFailure" />
+                </Control>
             </Control>
             <SubTree ID="InterpolateToJointStateWaypoint" target_joint_state="{target_joint_state}"/>
         </Control>
diff --git a/src/picknik_ur_base_config/objectives/move_to_known_pose.xml b/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
index 532c135c..ca741bcc 100644
--- a/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
+++ b/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
@@ -5,7 +5,10 @@
             <!-- When the user clicks on different pose buttons on Endpoint tab, the frontend sets move_to_known_pose.target_state_name parameter value-->
             <Control ID="Fallback" name="log_error_if_parameter_not_set">
                 <Action ID="GetStringFromUser" parameter_name="move_to_known_pose.target_state_name" parameter_value="{target_state_name}"/>
-                <Action ID="LogMessage" message="This objective is used by Move To Pose buttons in the Endpoint tab." log_level="error" />
+                <Control ID="Sequence">
+                    <Action ID="LogMessage" message="This objective is meant to be used by Move To Pose buttons in the Endpoint tab. Try it out there!" log_level="error" />
+                    <Action ID="AlwaysFailure" />
+                </Control>
             </Control>
             <SubTree ID="MoveToWaypoint" waypoint_name="{target_state_name}"/>
         </Control>

From 7b7e0b6d6052a06ee6676b67f5d46d4756504978 Mon Sep 17 00:00:00 2001
From: Abishalini Sivaraman <abi.gpuram@gmail.com>
Date: Wed, 7 Jun 2023 12:43:28 -0600
Subject: [PATCH 6/7] Update
 src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
---
 .../objectives/interpolate_to_joint_state.xml                   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml b/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
index cb0c2828..a2ea97c9 100644
--- a/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
+++ b/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml
@@ -5,7 +5,7 @@
             <Control ID="Fallback" name="log_error_if_parameter_not_set">
                 <Action ID="GetJointStateFromUser" parameter_name="interpolate_to_joint_state.target_joint_state" parameter_value="{target_joint_state}" />
                 <Control ID="Sequence">
-                    <Action ID="LogMessage" message="This objective is meant to be used by joint sliders in the Joints tab. Try it out there!" log_level="error" />
+                    <Action ID="LogMessage" message="This Objective is meant to be used by joint sliders in the Joints tab. Try it out there!" log_level="error" />
                     <Action ID="AlwaysFailure" />
                 </Control>
             </Control>

From c6d0054d37391ea2c96db081caf7ba4356532adc Mon Sep 17 00:00:00 2001
From: Abishalini Sivaraman <abi.gpuram@gmail.com>
Date: Wed, 7 Jun 2023 12:43:40 -0600
Subject: [PATCH 7/7] Update
 src/picknik_ur_base_config/objectives/move_to_known_pose.xml

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
---
 src/picknik_ur_base_config/objectives/move_to_known_pose.xml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/picknik_ur_base_config/objectives/move_to_known_pose.xml b/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
index ca741bcc..fe3f65f5 100644
--- a/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
+++ b/src/picknik_ur_base_config/objectives/move_to_known_pose.xml
@@ -6,7 +6,7 @@
             <Control ID="Fallback" name="log_error_if_parameter_not_set">
                 <Action ID="GetStringFromUser" parameter_name="move_to_known_pose.target_state_name" parameter_value="{target_state_name}"/>
                 <Control ID="Sequence">
-                    <Action ID="LogMessage" message="This objective is meant to be used by Move To Pose buttons in the Endpoint tab. Try it out there!" log_level="error" />
+                    <Action ID="LogMessage" message="This Objective is meant to be used by Move To Pose buttons in the Endpoint tab. Try it out there!" log_level="error" />
                     <Action ID="AlwaysFailure" />
                 </Control>
             </Control>