add grasp planning example showcasing MTC batch IK

src/lab_sim/objectives/grasp_planning.xml

@@ -0,0 +1,113 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Grasp Planning">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Grasp Planning"
+    _description="Find a feasible grasp pose out of a set of manually-defined grasp candidates, and move there"
+    _favorite="false"
+    _subtreeOnly="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree ID="Clear Snapshot" _collapsed="true" />
+      <SubTree ID="Look at table" _collapsed="true" />
+      <!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Open Gripper" _collapsed="true" />
+      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
+      <Control ID="Sequence">
+        <!--Define and visualize two grasp candidates-->
+        <Action
+          ID="CreateStampedPose"
+          orientation_xyzw="0;1;0;0"
+          position_xyz="0.01;.75;0.515"
+          stamped_pose="{grasp_candidate_1}"
+          reference_frame="world"
+        />
+        <Action
+          ID="AddPoseStampedToVector"
+          input="{grasp_candidate_1}"
+          vector="{grasp_candidates}"
+        />
+        <Action
+          ID="VisualizePose"
+          marker_lifetime="0.000000"
+          marker_name="grasp_candidate_1"
+          marker_size="0.100000"
+          pose="{grasp_candidate_1}"
+        />
+        <Action
+          ID="CreateStampedPose"
+          orientation_xyzw="0;0.9848078;0.1736482;0"
+          position_xyz="0.01;.75;0.515"
+          stamped_pose="{grasp_candidate_2}"
+          reference_frame="world"
+        />
+        <Action
+          ID="AddPoseStampedToVector"
+          input="{grasp_candidate_2}"
+          vector="{grasp_candidates}"
+        />
+        <Action
+          ID="VisualizePose"
+          marker_lifetime="0.000000"
+          marker_name="grasp_candidate_2"
+          marker_size="0.100000"
+          pose="{grasp_candidate_2}"
+        />
+      </Control>
+      <Control ID="Sequence">
+        <!--Find a feasible grasp pose and move there-->
+        <Action
+          ID="InitializeMTCTask"
+          task_id="pick_object"
+          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+          task="{pick_object_task}"
+        />
+        <Action
+          ID="SetupMTCCurrentState"
+          task="{pick_object_task}"
+          skip_collision_check="false"
+        />
+        <Action
+          ID="SetupMTCConnectWithTrajectory"
+          constraints="{constraints}"
+          planner_interface="moveit_default"
+          planning_group_name="manipulator"
+          task="{pick_object_task}"
+        />
+        <Action
+          ID="SetupMTCBatchPoseIK"
+          end_effector_group="moveit_ee"
+          end_effector_link="grasp_link"
+          ik_timeout_s="0.01"
+          max_ik_solutions="1"
+          monitored_stage="current state"
+          target_poses="{grasp_candidates}"
+          task="{pick_object_task}"
+        />
+        <Action
+          ID="PlanMTCTask"
+          solution="{pick_object_solution}"
+          task="{pick_object_task}"
+        />
+        <SubTree
+          ID="Wait for Trajectory Approval if User Available"
+          solution="{pick_object_solution}"
+        />
+        <Action
+          ID="ExecuteMTCTask"
+          solution="{pick_object_solution}"
+          goal_duration_tolerance="-1.000000"
+        />
+      </Control>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Grasp Planning">
+      <MetadataFields>
+        <Metadata subcategory="Motion - Planning" />
+        <Metadata runnable="true" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>