From 41a01e9a3f022b3b23906ffffa56579311dbd31e Mon Sep 17 00:00:00 2001
From: rlpratt12 <richard.pratt@picknik.ai>
Date: Fri, 15 May 2026 14:40:16 -0600
Subject: [PATCH 1/2] fix(lab_sim): Tutorial 1 walk-through fixes

- ml_find_objects_on_table.xml: Add SwitchUIPrimaryView at the end of
  the tree so a view pane auto-switches to /masks_visualization once
  masks are published. Matches the behavior of the other ML Objectives
  in lab_sim (ml_segment_image,
  ml_find_bottles_on_table_from_image_exemplar) and restores the
  auto-show readers expect from Tutorial 1.
- pick_all_bottles_with_apriltags.xml: Flatten the "Release and Retract"
  subtree, which had three nested single-child Sequence wrappers around
  the actual actions. The wrappers were structurally inert but very
  confusing when a tutorial reader opens the tree.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
---
 .../objectives/ml_find_objects_on_table.xml   |   5 +
 .../pick_all_bottles_with_apriltags.xml       | 106 +++++++++---------
 2 files changed, 55 insertions(+), 56 deletions(-)

diff --git a/src/lab_sim/objectives/ml_find_objects_on_table.xml b/src/lab_sim/objectives/ml_find_objects_on_table.xml
index 44e6d0b45..b5f87c527 100644
--- a/src/lab_sim/objectives/ml_find_objects_on_table.xml
+++ b/src/lab_sim/objectives/ml_find_objects_on_table.xml
@@ -26,6 +26,11 @@
         bounding_box_labels="{confidence_scores_str}"
         image="{wrist_camera_image}"
       />
+      <!--Auto-switch a view pane to the masks visualization so users can immediately see the output.-->
+      <Action
+        ID="SwitchUIPrimaryView"
+        primary_view_name="/masks_visualization"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml b/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml
index 5a18620f7..15cf98c7a 100644
--- a/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml
+++ b/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml
@@ -353,62 +353,56 @@
               name="Release and Retract"
               _collapsed="false"
             >
-              <Control ID="Sequence" _collapsed="false">
-                <Control ID="Sequence" _collapsed="false">
-                  <Control ID="Sequence" _collapsed="false">
-                    <!--Release the bottle-->
-                    <SubTree ID="Open Gripper" _collapsed="true" />
-                    <!--Retract upward after releasing the bottle-->
-                    <Action
-                      ID="CreatePoseStamped"
-                      reference_frame="grasp_link"
-                      pose_stamped="{place_retract_pose}"
-                      position_xyz="0;0;-0.08"
-                      orientation_xyzw="0.000000;0.000000;0.000000;1.000000"
-                    />
-                    <Action
-                      ID="ResetPoseStampedVector"
-                      vector="{place_retract_path}"
-                    />
-                    <Action
-                      ID="AddPoseStampedToVector"
-                      input="{place_retract_pose}"
-                      vector="{place_retract_path}"
-                    />
-                    <Action
-                      ID="PlanCartesianPath"
-                      acceleration_scale_factor="1.000000"
-                      blending_radius="0.020000"
-                      debug_solution="{debug_solution}"
-                      ik_cartesian_space_density="0.010000"
-                      ik_joint_space_density="0.100000"
-                      joint_trajectory_msg="{joint_trajectory_msg}"
-                      path="{place_retract_path}"
-                      planning_group_name="manipulator"
-                      position_only="true"
-                      tip_links="grasp_link"
-                      trajectory_sampling_rate="100"
-                      velocity_scale_factor="1.000000"
-                      max_optimizer_iterations="1"
-                      tip_offset="0.000000;0.000000;0.000000;0.000000;0.000000;0.000000"
-                    />
-                    <Action
-                      ID="ExecuteTrajectory"
-                      goal_duration_tolerance="-1.000000"
-                      joint_trajectory_msg="{joint_trajectory_msg}"
-                      controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
-                      execution_pipeline="jtac"
-                    />
-                    <!--Remove the used pose so the next iteration gets a fresh position-->
-                    <Action
-                      ID="RemoveFromVector"
-                      input_vector="{place_positions}"
-                      index="0"
-                      output_vector="{place_positions}"
-                    />
-                  </Control>
-                </Control>
-              </Control>
+              <!--Release the bottle-->
+              <SubTree ID="Open Gripper" _collapsed="true" />
+              <!--Retract upward after releasing the bottle-->
+              <Action
+                ID="CreatePoseStamped"
+                reference_frame="grasp_link"
+                pose_stamped="{place_retract_pose}"
+                position_xyz="0;0;-0.08"
+                orientation_xyzw="0.000000;0.000000;0.000000;1.000000"
+              />
+              <Action
+                ID="ResetPoseStampedVector"
+                vector="{place_retract_path}"
+              />
+              <Action
+                ID="AddPoseStampedToVector"
+                input="{place_retract_pose}"
+                vector="{place_retract_path}"
+              />
+              <Action
+                ID="PlanCartesianPath"
+                acceleration_scale_factor="1.000000"
+                blending_radius="0.020000"
+                debug_solution="{debug_solution}"
+                ik_cartesian_space_density="0.010000"
+                ik_joint_space_density="0.100000"
+                joint_trajectory_msg="{joint_trajectory_msg}"
+                path="{place_retract_path}"
+                planning_group_name="manipulator"
+                position_only="true"
+                tip_links="grasp_link"
+                trajectory_sampling_rate="100"
+                velocity_scale_factor="1.000000"
+                max_optimizer_iterations="1"
+                tip_offset="0.000000;0.000000;0.000000;0.000000;0.000000;0.000000"
+              />
+              <Action
+                ID="ExecuteTrajectory"
+                goal_duration_tolerance="-1.000000"
+                joint_trajectory_msg="{joint_trajectory_msg}"
+                controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
+                execution_pipeline="jtac"
+              />
+              <!--Remove the used pose so the next iteration gets a fresh position-->
+              <Action
+                ID="RemoveFromVector"
+                input_vector="{place_positions}"
+                index="0"
+                output_vector="{place_positions}"
+              />
             </Control>
           </Control>
         </Decorator>

From 20bce2f2b636e97321bd92d326e0439ac6a60a6c Mon Sep 17 00:00:00 2001
From: rlpratt12 <richard.pratt@picknik.ai>
Date: Fri, 15 May 2026 15:18:46 -0600
Subject: [PATCH 2/2] fix(lab_sim): Remove warm-up velocity_force_controller
 block

Drop the warm-up preamble (2x SwitchController + zero_fts) at the top of
the Pick All Bottles with AprilTags tree. The cold-start cost it was
guarding against is essentially zero in the current build, and the block
adds ~3-4s of unnecessary controller-swap latency at the top of every
run.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
---
 .../pick_all_bottles_with_apriltags.xml       | 33 -------------------
 1 file changed, 33 deletions(-)

diff --git a/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml b/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml
index 15cf98c7a..9933416d1 100644
--- a/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml
+++ b/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml
@@ -123,39 +123,6 @@
         <SubTree ID="Look at Table" _collapsed="true" />
         <SubTree ID="Take Wrist Camera Snapshot" _collapsed="true" />
       </Control>
-      <!--Warm up velocity_force_controller so the first placement is not a cold start-->
-      <Control ID="Sequence" _collapsed="false">
-        <Action
-          ID="SwitchController"
-          activate_controllers="velocity_force_controller"
-          controller_switch_action_name="/controller_manager/switch_controller"
-          timeout="-1.000000"
-          activate_asap="true"
-          strictness="1"
-          controller_list_action_name="/controller_manager/list_controllers"
-          automatic_deactivation="true"
-          automatic_activation="true"
-          deactivate_controllers=""
-        />
-        <Action
-          ID="CallTriggerService"
-          service_name="/velocity_force_controller/zero_fts"
-          response_timeout="3.000000"
-          wait_for_server_available_timeout="3.000000"
-        />
-        <Action
-          ID="SwitchController"
-          activate_controllers="joint_trajectory_admittance_controller"
-          controller_switch_action_name="/controller_manager/switch_controller"
-          timeout="-1.000000"
-          activate_asap="true"
-          strictness="1"
-          controller_list_action_name="/controller_manager/list_controllers"
-          automatic_deactivation="true"
-          automatic_activation="true"
-          deactivate_controllers=""
-        />
-      </Control>
       <Decorator ID="ForceSuccess" _collapsed="false">
         <Decorator ID="KeepRunningUntilFailure" _collapsed="false">
           <Control ID="Sequence" name="TopLevelSequence" _collapsed="false">