From b8294cf6d01907dd9401d1ea463f35d25dc97d0d Mon Sep 17 00:00:00 2001
From: Dave Coleman <dave@picknik.ai>
Date: Wed, 10 Jul 2024 13:20:37 -0600
Subject: [PATCH 1/2] Improve inline documentation for constrained_pick_place

---
 .../objectives/constrained_pick_place.xml     | 25 +++++++++++--------
 1 file changed, 14 insertions(+), 11 deletions(-)

diff --git a/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml b/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml
index 72bc93e1..61ea789b 100644
--- a/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml
+++ b/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml
@@ -1,21 +1,24 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Constrained Pick and Place">
   <!-- ////////// -->
-  <BehaviorTree ID="Constrained Pick and Place" _description="Pick and place an object with constraints on the gripper's range of motion" _favorite="true">
+  <BehaviorTree ID="Constrained Pick and Place" _description="More advanced example of picking and placing an (invisible) object with orientation constraints enforced on the gripper's motion" _favorite="true">
     <Decorator ID="KeepRunningUntilFailure">
       <Control ID="Sequence">
+        <!--Create a special type of motion planning configuration that includes an upwards orientation requirement-->
         <Action ID="InitializeMotionConstraints" constraints_name="gripper pointing down" constraints="{constraints}"/>
         <Action ID="AppendOrientationConstraint" config_file_name="my_constraints.yaml" constraints="{constraints}"/>
-        <Control ID="Sequence" name="TopLevelSequence">
-          <SubTree ID="Open Gripper"/>
-          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <SubTree ID="Move to Waypoint" waypoint_name="Grasp Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
-          </Decorator>
-          <SubTree ID="Close Gripper"/>
-          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" waypoint_name="Grasp Left" planner_interface="moveit_all_planners" constraints="{constraints}"/>
-          </Decorator>
-        </Control>
+        <!--Move to pick location-->
+        <SubTree ID="Open Gripper"/>
+        <!--Note: sometimes constraint planning can fail, so we add a retry decorator to improve success-->
+        <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+          <SubTree ID="Move to Waypoint" waypoint_name="Grasp Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
+        </Decorator>
+        <!--Pick-->
+        <SubTree ID="Close Gripper"/>
+        <!--Move to place (drop) location-->
+        <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+          <SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" waypoint_name="Grasp Left" planner_interface="moveit_default" constraints="{constraints}"/>
+        </Decorator>
       </Control>
     </Decorator>
   </BehaviorTree>

From 9c3dcd32e5614b328f16b16b28e66a4468e66016 Mon Sep 17 00:00:00 2001
From: Dave Coleman <dave@picknik.ai>
Date: Wed, 10 Jul 2024 17:07:09 -0600
Subject: [PATCH 2/2] Update
 src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml

Co-authored-by: Michael Wrock <mike.wrock@gmail.com>
---
 .../objectives/constrained_pick_place.xml                       | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml b/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml
index 61ea789b..d41f44de 100644
--- a/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml
+++ b/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml
@@ -9,7 +9,7 @@
         <Action ID="AppendOrientationConstraint" config_file_name="my_constraints.yaml" constraints="{constraints}"/>
         <!--Move to pick location-->
         <SubTree ID="Open Gripper"/>
-        <!--Note: sometimes constraint planning can fail, so we add a retry decorator to improve success-->
+        <!--Note: Sampling based planners can be non-deterministic. The retry decorator improves the likelihood of success-->
         <Decorator ID="RetryUntilSuccessful" num_attempts="3">
           <SubTree ID="Move to Waypoint" waypoint_name="Grasp Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
         </Decorator>