Merge pull request #1 from PickNikRobotics/add-custom-pick-and-place-example

Add pick and place example using custom MTC example behavior

Author: MikeWrock

src/lab_sim/config/config.yaml

@@ -43,6 +43,7 @@ objectives:
       - "moveit_studio::behaviors::ServoBehaviorsLoader"
       - "moveit_studio::behaviors::VisionBehaviorsLoader"
       - "picknik_registration::PicknikRegistrationBehaviorsLoader"
+      - "example_behaviors::ExampleBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:

src/lab_sim/objectives/pick_and_place_example.xml

@@ -0,0 +1,25 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Pick And Place Example">
+  <!-- ////////// -->
+  <BehaviorTree ID="Pick And Place Example" _description="Pick and place example objective" _favorite="true" _hardcoded="false">
+    <Control ID="Sequence" name="Pick and Place">
+      <SubTree ID="Open Gripper"/>
+      <Control ID="Sequence" name="Pick">
+        <Action ID="CreateStampedPose" reference_frame="world" position_xyz="0.01;.75;0.515" orientation_xyzw="0;1;0;0" stamped_pose="{pick_pose}"/>
+        <Action ID="InitializeMTCTask" task_id="pick_task" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{mtc_pick_task}"/>
+        <Action ID="SetupMTCCurrentState" task="{mtc_pick_task}"/>
+        <Action ID="SetupMtcPickFromPose" grasp_pose="{pick_pose}" task="{mtc_pick_task}"/>
+        <Action ID="PlanMTCTask" task="{mtc_pick_task}" solution="{mtc_pick_solution}"/>
+        <Action ID="ExecuteMTCTask" solution="{mtc_pick_solution}"/>
+      </Control>
+      <Control ID="Sequence" name="Place">
+        <Action ID="InitializeMTCTask" task_id="place_task" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{mtc_place_task}"/>
+        <Action ID="SetupMTCCurrentState" task="{mtc_place_task}"/>
+        <Action ID="CreateStampedPose" reference_frame="world" position_xyz="0.01;.75;0.5.15" orientation_xyzw="0;1;0;0" stamped_pose="{place_pose}"/>
+        <Action ID="SetupMtcPlaceFromPose" place_pose="{place_pose}" task="{mtc_place_task}"/>
+        <Action ID="PlanMTCTask" task="{mtc_place_task}" solution="{mtc_place_solution}"/>
+        <Action ID="ExecuteMTCTask" solution="{mtc_place_solution}"/>
+      </Control>
+    </Control>
+  </BehaviorTree>
+</root>