Improve lab sim example objectives with story telling

src/lab_sim/config/moveit/picknik_ur.srdf

@@ -52,6 +52,7 @@
     <disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Adjacent"/>
     <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent"/>
     <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link_inertia" link2="upper_arm_link" reason="Adjacent"/>
     <disable_collisions link1="forearm_link" link2="wrist_2_link" reason="Never"/>
     <disable_collisions link1="forearm_pinch_link" link2="forearm_link" reason="Adjacent"/>
     <disable_collisions link1="forearm_pinch_link" link2="tool_changer_link" reason="Never"/>

src/lab_sim/description/picknik_ur.xacro

@@ -1202,6 +1202,7 @@
       <param name="render_publish_rate">10</param>
       <param name="tf_publish_rate">60</param>
       <param name="lidar_publish_rate">10</param>
+      <param name="mujoco_viewer">false</param>
     </hardware>
   </ros2_control>
 </robot>

src/lab_sim/objectives/3_waypoint_pick_and_place.xml

@@ -1,13 +1,12 @@
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="3 Waypoints Pick and Place">
   <!--//////////-->
   <BehaviorTree
     ID="3 Waypoints Pick and Place"
     _description="Basic example of repeatedly grabbing a small (invisible) object, placing it at desired destination, and then moving back to a home position"
-    _favorite="false"
+    _favorite="true"
   >
     <Control ID="Sequence">
-      <!--Reset the planning scene to ensure no old collision objects are hanging around-->
-      <Action ID="ClearSnapshot" />
       <!--Setup the environment to its "home" configuration - move to center and open gripper-->
       <SubTree
         ID="Move to Waypoint"

src/lab_sim/objectives/_scan_scene.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="_Scan Scene">
+  <!--//////////-->
+  <BehaviorTree
+    ID="_Scan Scene"
+    _description="Move robot our of scene to enable full snapshot of scene."
+    _favorite="true"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="ClearSnapshot" />
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        joint_group_name="manipulator"
+        planner_interface="moveit_default"
+        controller_names="/joint_trajectory_controller"
+        waypoint_name="Park Far Right"
+      />
+      <SubTree ID="Take scene camera snapshot" _collapsed="true" />
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        joint_group_name="manipulator"
+        planner_interface="moveit_default"
+        controller_names="/joint_trajectory_controller"
+        waypoint_name="Look at Table"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="_Scan Scene" />
+  </TreeNodesModel>
+</root>

src/lab_sim/objectives/constrained_pick_and_place_subtree.xml

@@ -0,0 +1,106 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<root
+  BTCPP_format="4"
+  main_tree_to_execute="Constrained Pick and Place Subtree"
+>
+  <!--//////////-->
+  <BehaviorTree
+    ID="Constrained Pick and Place Subtree"
+    _subtreeOnly="true"
+    _description=""
+  >
+    <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}"
+      />
+      <!--Move to pick location-->
+      <!--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="{pre_pick}"
+          joint_group_name="manipulator"
+          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+          planner_interface="moveit_default"
+          constraints="{constraints}"
+        />
+      </Decorator>
+      <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+        <SubTree
+          ID="Move to Waypoint"
+          waypoint_name="{pick}"
+          joint_group_name="manipulator"
+          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+          planner_interface="moveit_default"
+          constraints="{constraints}"
+        />
+      </Decorator>
+      <!--We force success as the gripper closes, since we are commanding a position it will never reach (fingers fully closed)-->
+      <Decorator ID="ForceSuccess">
+        <SubTree ID="Close Gripper" />
+      </Decorator>
+      <!--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"
+          planner_interface="moveit_default"
+          constraints="{constraints}"
+          _collapsed="true"
+          waypoint_name="{pre_pick}"
+        />
+      </Decorator>
+      <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+        <SubTree
+          ID="Move to Waypoint"
+          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+          joint_group_name="manipulator"
+          planner_interface="moveit_default"
+          constraints="{constraints}"
+          _collapsed="true"
+          waypoint_name="{pre_place}"
+        />
+      </Decorator>
+      <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+        <SubTree
+          ID="Move to Waypoint"
+          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+          joint_group_name="manipulator"
+          planner_interface="moveit_default"
+          constraints="{constraints}"
+          _collapsed="true"
+          waypoint_name="{place}"
+        />
+      </Decorator>
+      <SubTree ID="Open Gripper" />
+      <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+        <SubTree
+          ID="Move to Waypoint"
+          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+          joint_group_name="manipulator"
+          planner_interface="moveit_default"
+          constraints="{constraints}"
+          _collapsed="true"
+          waypoint_name="{pre_place}"
+        />
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Constrained Pick and Place Subtree">
+      <inout_port name="pre_pick" default="" />
+      <inout_port name="pick" default="" />
+      <inout_port name="pre_place" default="" />
+      <inout_port name="place" default="" />
+    </SubTree>
+  </TreeNodesModel>
+</root>