Add Collect Training Data with Behaviors Objective to lab_sim

src/lab_sim/objectives/collect_training_data_with_behaviors.xml

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root
+  BTCPP_format="4"
+  main_tree_to_execute="Collect Training Data with Behaviors"
+>
+  <BehaviorTree
+    ID="Collect Training Data with Behaviors"
+    _description="Wraps the Pick April Tag Labeled Object task with data-recording Behaviors: starts a Trainer recording, runs the task, saves the successful episode, and converts the dataset to LeRobotDataset v3.0. A failed pick is discarded and the Objective fails."
+    _favorite="false"
+  >
+    <Control ID="Fallback" name="RecordWithCleanup">
+      <Control ID="Sequence" name="RecordAndConvert">
+        <Action
+          ID="RecordEpisode"
+          name="Start Trainer recording"
+          dataset_name="behavior_bottle_pick"
+          task="Pick up an AprilTag-labeled bottle."
+          config_name="lab_sim"
+          num_episodes="1"
+        />
+        <!--Let the recording settle before motion begins.-->
+        <Action
+          ID="WaitForDuration"
+          name="Let recording settle"
+          delay_duration="2.0"
+        />
+        <SubTree ID="Pick April Tag Labeled Object" _collapsed="true" />
+        <Action ID="SaveEpisode" name="Keep successful demonstration" />
+        <!--Let the saved episode finish updating in Trainer before the
+             session closes.-->
+        <Action
+          ID="WaitForDuration"
+          name="Let episode finish saving"
+          delay_duration="1.0"
+        />
+        <Action ID="StopRecording" name="Finish recording session" />
+        <Action
+          ID="ConvertDataset"
+          name="Convert to LeRobot dataset"
+          dataset_name="behavior_bottle_pick"
+        />
+      </Control>
+      <!--Cleanup, not a forced success: if the task fails before SaveEpisode,
+           StopRecording discards the unsaved attempt and AlwaysFailure
+           preserves the Objective's failed result, so a failed pick is never
+           labeled as a successful demonstration.-->
+      <Control ID="Sequence" name="DiscardFailedAttempt">
+        <Action ID="StopRecording" name="Discard unsaved attempt" />
+        <Action ID="AlwaysFailure" />
+      </Control>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Collect Training Data with Behaviors">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>

src/lab_sim/objectives/pick_from_pose.xml

@@ -83,7 +83,7 @@
         joint_trajectory_msg="{joint_trajectory_msg}"
         path="{retract_path}"
         planning_group_name="manipulator"
-        position_only="false"
+        cartesian_constraint="[{constraint_type: position_and_orientation}]"
         tip_links="grasp_link"
         trajectory_timing="[{mode: time_optimal}]"
       />

src/lab_sim/test/objectives_integration_test.py

@@ -274,6 +274,7 @@ def _controller_safe_mujoco_reset(
 # Objectives to skip entirely from integration testing
 skip_objectives = {
     "AddBottlesToPlanningScene",
+    "Collect Training Data with Behaviors",  # Requires setup for data collection (recording infrastructure not available in CI)
     "Grasp Planning",
     "Joint Diagnostic",
     "ML Find Bottles on Table from Image Exemplar",  # Skipped because it looks for a file on a home path