Changes to loop ConstrainedPickandPlace objective (#105)

Added in a KeepRunningUntilFailure decorator at the top of the objective
so it will loop.

Made the objective go from Grasp Right -> Grasp Left waypoint
rather than Grasp Left -> Gripper Down -> Grasp Right:
* If we leave the Gripper Down waypoint in between, then we don't end up
  doing constrained planning. The straight-line path between Grasp Left
  and Gripper Down in the joint space also satisfies the constraint, so
  constrained planning is not needed.
* From trial and error, doing constrained planning from Grasp Right ->
  Grasp Left succeeds much more frequently then the other way around.
  Likely due to the joint orientations of the UR5 arm.

Added in RetryUntilSuccessful decorators in front of the MoveToWaypoint
behaviors since those fail occaisionally due to collisions that pop up
during path simplification.

src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml

@@ -2,14 +2,21 @@
 <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">
-    <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="MoveToWaypoint" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
-      <Action ID="InitializeMotionConstraints" constraints_name="gripper pointing down" constraints="{constraints}"/>
-      <Action ID="AppendOrientationConstraint" config_file_name="my_constraints.yaml" constraints="{constraints}"/>
-      <SubTree ID="Open Gripper"/>
-      <Action ID="MoveToWaypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Gripper Down" use_all_planners="true" constraints="{constraints}"/>
-      <Action ID="MoveToWaypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Grasp Right" use_all_planners="true" constraints="{constraints}"/>
-      <SubTree ID="Close Gripper"/>
-    </Control>
+    <Decorator ID="KeepRunningUntilFailure">
+      <Control ID="Sequence">
+        <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">
+            <Action ID="MoveToWaypoint" waypoint_name="Grasp Right" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
+          </Decorator>
+          <SubTree ID="Close Gripper"/>
+          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+            <Action ID="MoveToWaypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Grasp Left" use_all_planners="true" constraints="{constraints}"/>
+          </Decorator>
+        </Control>
+      </Control>
+    </Decorator>
   </BehaviorTree>
 </root>