From 9307b0c4f8136aff8552839cb86171dfc59b1f04 Mon Sep 17 00:00:00 2001
From: Dave Coleman <dave@picknik.ai>
Date: Wed, 4 Mar 2026 17:32:29 -0700
Subject: [PATCH 1/3] Clean up and improve lab_sim objectives

- Wrap Open/Close Gripper in ForceSuccess instead of Sequence
- Replace _Scan Scene and Scan Multiple Views with Scan Scene - Multiple Point Clouds
- Rename Stack Blocks with ICP to Stack Objects with ICP
- Rename show_grasp_link to Visualize Grasp Link
- Remove Pick And Place Example (superseded)
- Recategorize pick/place subtrees to Manipulation
- Remove redundant _favorite="false" from subtrees
- Make Move Along Square runnable

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
---
 src/lab_sim/objectives/_scan_scene.xml        |  34 -----
 src/lab_sim/objectives/close_gripper.xml      |   4 +-
 ...nt.xml => computelinkposefromwaypoint.xml} |  10 +-
 src/lab_sim/objectives/grasp_planning.xml     |   2 +-
 src/lab_sim/objectives/move_along_square.xml  |   2 +-
 src/lab_sim/objectives/mpc_pose_tracking.xml  |   2 +-
 ...th_static_sphere_point_cloud_avoidance.xml |   2 +-
 src/lab_sim/objectives/open_gripper.xml       |   6 +-
 .../objectives/pick_and_place_example.xml     |  44 -------
 src/lab_sim/objectives/pick_from_pose.xml     |   2 +-
 .../pick_from_pose_with_approval.xml          |   2 +-
 src/lab_sim/objectives/pick_up_cube.xml       |  10 +-
 src/lab_sim/objectives/place_at_pose.xml      |   2 +-
 .../place_at_pose_with_approval.xml           |   2 +-
 .../objectives/scan_multiple_views.xml        |  26 ----
 .../scan_scene_-_multiple_point_clouds.xml    | 121 ++++++++++++++++++
 .../objectives/stack_blocks_with_icp.xml      |   6 +-
 ...rasp_link.xml => visualize_grasp_link.xml} |   6 +-
 18 files changed, 152 insertions(+), 131 deletions(-)
 delete mode 100644 src/lab_sim/objectives/_scan_scene.xml
 rename src/lab_sim/objectives/{compute_linkpose_from_waypoint.xml => computelinkposefromwaypoint.xml} (88%)
 delete mode 100644 src/lab_sim/objectives/pick_and_place_example.xml
 delete mode 100644 src/lab_sim/objectives/scan_multiple_views.xml
 create mode 100644 src/lab_sim/objectives/scan_scene_-_multiple_point_clouds.xml
 rename src/lab_sim/objectives/{show_grasp_link.xml => visualize_grasp_link.xml} (82%)

diff --git a/src/lab_sim/objectives/_scan_scene.xml b/src/lab_sim/objectives/_scan_scene.xml
deleted file mode 100644
index 334a261c8..000000000
--- a/src/lab_sim/objectives/_scan_scene.xml
+++ /dev/null
@@ -1,34 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="_Scan Scene">
-  <!--//////////-->
-  <BehaviorTree
-    ID="_Scan Scene"
-    _description="Move robot out 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"
-        waypoint_name="Park Far Right"
-      />
-      <SubTree ID="Take Scene Camera Snapshot" _collapsed="true" />
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        joint_group_name="manipulator"
-        waypoint_name="Look at Table"
-      />
-    </Control>
-  </BehaviorTree>
-  <TreeNodesModel>
-    <SubTree ID="_Scan Scene">
-      <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Basic Examples" />
-      </MetadataFields>
-    </SubTree>
-  </TreeNodesModel>
-</root>
diff --git a/src/lab_sim/objectives/close_gripper.xml b/src/lab_sim/objectives/close_gripper.xml
index 6960f3d7f..6e0a3e6dd 100644
--- a/src/lab_sim/objectives/close_gripper.xml
+++ b/src/lab_sim/objectives/close_gripper.xml
@@ -5,14 +5,14 @@
     _description="Close the gripper"
     _favorite="false"
   >
-    <Control ID="Sequence" name="root">
+    <Decorator ID="ForceSuccess">
       <Action
         ID="MoveGripperAction"
         gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
         position="0.7929"
         timeout="15.000000"
       />
-    </Control>
+    </Decorator>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Close Gripper">
diff --git a/src/lab_sim/objectives/compute_linkpose_from_waypoint.xml b/src/lab_sim/objectives/computelinkposefromwaypoint.xml
similarity index 88%
rename from src/lab_sim/objectives/compute_linkpose_from_waypoint.xml
rename to src/lab_sim/objectives/computelinkposefromwaypoint.xml
index a231afb76..14ee2e031 100644
--- a/src/lab_sim/objectives/compute_linkpose_from_waypoint.xml
+++ b/src/lab_sim/objectives/computelinkposefromwaypoint.xml
@@ -1,13 +1,17 @@
 <?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="ComputeLinkPoseFromWaypoint">
   <!--//////////-->
-  <BehaviorTree ID="ComputeLinkPoseFromWaypoint" _favorite="false">
+  <BehaviorTree
+    ID="ComputeLinkPoseFromWaypoint"
+    _favorite="false"
+    _description=""
+  >
     <Control ID="Sequence" _collapsed="false">
       <Action
         ID="RetrieveWaypoint"
         joint_group_name="manipulator"
         waypoint_joint_state="{target_joint_state}"
-        waypoint_name="Above Pick Cube"
+        waypoint_name="Above Pick Object"
       />
       <Action
         ID="UnpackRobotJointStateMessage"
@@ -32,7 +36,7 @@
     <SubTree ID="ComputeLinkPoseFromWaypoint">
       <MetadataFields>
         <Metadata runnable="false" />
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Manipulation" />
         <Metadata
           description="This objective retrieves a named waypoint, unpacks the joint state, gets the current planning scene, and computes the pose of a specified link using forward kinematics. The resulting pose is output as a geometry_msgs::msg::PoseStamped."
         />
diff --git a/src/lab_sim/objectives/grasp_planning.xml b/src/lab_sim/objectives/grasp_planning.xml
index 34038f2e4..597d3ca41 100644
--- a/src/lab_sim/objectives/grasp_planning.xml
+++ b/src/lab_sim/objectives/grasp_planning.xml
@@ -9,7 +9,7 @@
     <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-->
+      <!--Take a snapshot so we have a visualization of the objects 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" />
diff --git a/src/lab_sim/objectives/move_along_square.xml b/src/lab_sim/objectives/move_along_square.xml
index 6c8b72fbd..c5292a813 100644
--- a/src/lab_sim/objectives/move_along_square.xml
+++ b/src/lab_sim/objectives/move_along_square.xml
@@ -83,7 +83,7 @@
   <TreeNodesModel>
     <SubTree ID="Move Along Square">
       <MetadataFields>
-        <Metadata runnable="false" />
+        <Metadata runnable="true" />
         <Metadata subcategory="Application - Basic Examples" />
       </MetadataFields>
     </SubTree>
diff --git a/src/lab_sim/objectives/mpc_pose_tracking.xml b/src/lab_sim/objectives/mpc_pose_tracking.xml
index fa7571e23..8ee08b702 100644
--- a/src/lab_sim/objectives/mpc_pose_tracking.xml
+++ b/src/lab_sim/objectives/mpc_pose_tracking.xml
@@ -12,7 +12,7 @@
           ID="Move to Waypoint"
           _collapsed="true"
           joint_group_name="manipulator"
-          waypoint_name="Above Pick Cube"
+          waypoint_name="Above Pick Object"
         />
         <Action
           ID="CreateStampedPose"
diff --git a/src/lab_sim/objectives/mpc_pose_tracking_with_static_sphere_point_cloud_avoidance.xml b/src/lab_sim/objectives/mpc_pose_tracking_with_static_sphere_point_cloud_avoidance.xml
index b468d3a74..eedf4f290 100644
--- a/src/lab_sim/objectives/mpc_pose_tracking_with_static_sphere_point_cloud_avoidance.xml
+++ b/src/lab_sim/objectives/mpc_pose_tracking_with_static_sphere_point_cloud_avoidance.xml
@@ -10,7 +10,7 @@
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <SubTree ID="Clear Snapshot" _collapsed="true" />
-      <SubTree ID="_Scan Scene" _collapsed="true" />
+      <SubTree ID="Scan Scene - Multiple Point Clouds" _collapsed="true" />
       <Action
         ID="GetPointCloud"
         message_out="{point_cloud}"
diff --git a/src/lab_sim/objectives/open_gripper.xml b/src/lab_sim/objectives/open_gripper.xml
index a0509e5d7..7fda16316 100644
--- a/src/lab_sim/objectives/open_gripper.xml
+++ b/src/lab_sim/objectives/open_gripper.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Open Gripper">
   <!--//////////-->
   <BehaviorTree
@@ -6,14 +6,14 @@
     _description="Open the gripper"
     _favorite="false"
   >
-    <Control ID="Sequence" name="root">
+    <Decorator ID="ForceSuccess">
       <Action
         ID="MoveGripperAction"
         gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
         position="0.05"
         timeout="15.000000"
       />
-    </Control>
+    </Decorator>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Open Gripper">
diff --git a/src/lab_sim/objectives/pick_and_place_example.xml b/src/lab_sim/objectives/pick_and_place_example.xml
deleted file mode 100644
index 6d2ad6b28..000000000
--- a/src/lab_sim/objectives/pick_and_place_example.xml
+++ /dev/null
@@ -1,44 +0,0 @@
-<?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="Picks up a cube and then places it."
-    _favorite="false"
-    _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_place_pose}"
-        />
-        <SubTree
-          ID="Pick from Pose"
-          _collapsed="true"
-          grasp_pose="{pick_place_pose}"
-          approach_distance="0.1"
-          retract_xyz="0;0;-0.1"
-        />
-        <SubTree
-          ID="Place at Pose"
-          _collapsed="true"
-          place_pose="{pick_place_pose}"
-          approach_distance="0.1"
-          retract_xyz="0;0;-0.1"
-        />
-      </Control>
-    </Control>
-  </BehaviorTree>
-  <TreeNodesModel>
-    <SubTree ID="Pick And Place Example">
-      <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-      </MetadataFields>
-    </SubTree>
-  </TreeNodesModel>
-</root>
diff --git a/src/lab_sim/objectives/pick_from_pose.xml b/src/lab_sim/objectives/pick_from_pose.xml
index e73286bbc..320baa47a 100644
--- a/src/lab_sim/objectives/pick_from_pose.xml
+++ b/src/lab_sim/objectives/pick_from_pose.xml
@@ -98,7 +98,7 @@
   <TreeNodesModel>
     <SubTree ID="Pick from Pose">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Manipulation" />
         <Metadata runnable="false" />
       </MetadataFields>
       <inout_port name="approach_distance" default="0.1" />
diff --git a/src/lab_sim/objectives/pick_from_pose_with_approval.xml b/src/lab_sim/objectives/pick_from_pose_with_approval.xml
index aedfe8dae..dbfc65994 100644
--- a/src/lab_sim/objectives/pick_from_pose_with_approval.xml
+++ b/src/lab_sim/objectives/pick_from_pose_with_approval.xml
@@ -104,7 +104,7 @@
   <TreeNodesModel>
     <SubTree ID="Pick from Pose With Approval">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Manipulation" />
         <Metadata runnable="false" />
       </MetadataFields>
       <inout_port name="approach_distance" default="0.1" />
diff --git a/src/lab_sim/objectives/pick_up_cube.xml b/src/lab_sim/objectives/pick_up_cube.xml
index fad39d504..3186a75e9 100644
--- a/src/lab_sim/objectives/pick_up_cube.xml
+++ b/src/lab_sim/objectives/pick_up_cube.xml
@@ -1,21 +1,21 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Pick up Cube">
+<root BTCPP_format="4" main_tree_to_execute="Pick up Object">
   <!--//////////-->
   <BehaviorTree
-    ID="Pick up Cube"
-    _description="Pick up a cube from a manually defined location"
+    ID="Pick up Object"
+    _description="Pick up an object from a manually defined location"
     _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <SubTree ID="Clear Snapshot" _collapsed="true" />
       <SubTree ID="Look at Table" _collapsed="false" />
-      <!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
+      <!--Take a snapshot so we have a visualization of the objects to use in grasp pose correction-->
       <SubTree ID="Take Wrist Camera Snapshot" _collapsed="true" />
       <SubTree ID="Pick object" _collapsed="false" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Pick up Cube">
+    <SubTree ID="Pick up Object">
       <MetadataFields>
         <Metadata subcategory="Application - Basic Examples" />
         <Metadata runnable="true" />
diff --git a/src/lab_sim/objectives/place_at_pose.xml b/src/lab_sim/objectives/place_at_pose.xml
index 0862561b7..c50a95669 100644
--- a/src/lab_sim/objectives/place_at_pose.xml
+++ b/src/lab_sim/objectives/place_at_pose.xml
@@ -98,7 +98,7 @@
   <TreeNodesModel>
     <SubTree ID="Place at Pose">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Manipulation" />
         <Metadata runnable="false" />
       </MetadataFields>
       <inout_port name="approach_distance" default="0.1" />
diff --git a/src/lab_sim/objectives/place_at_pose_with_approval.xml b/src/lab_sim/objectives/place_at_pose_with_approval.xml
index 3ac6b6dca..981ac792a 100644
--- a/src/lab_sim/objectives/place_at_pose_with_approval.xml
+++ b/src/lab_sim/objectives/place_at_pose_with_approval.xml
@@ -97,7 +97,7 @@
   <TreeNodesModel>
     <SubTree ID="Place at Pose with Approval">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Manipulation" />
         <Metadata runnable="false" />
       </MetadataFields>
       <inout_port name="place_poses" default="{place_poses}" />
diff --git a/src/lab_sim/objectives/scan_multiple_views.xml b/src/lab_sim/objectives/scan_multiple_views.xml
deleted file mode 100644
index 16448fe6f..000000000
--- a/src/lab_sim/objectives/scan_multiple_views.xml
+++ /dev/null
@@ -1,26 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Scan Multiple Views">
-  <!--//////////-->
-  <BehaviorTree
-    ID="Scan Multiple Views"
-    _description="Take multiple snapshots from the wrist camera and merge them into a single point cloud."
-    _favorite="true"
-  >
-    <SubTree
-      ID="Fuse Multiple Views Subtree"
-      _collapsed="true"
-      waypoint1="Look at Objects - Top"
-      waypoint2="Look at Objects - Right"
-      waypoint3="Look at Objects - Left"
-      merged_cloud="merged_cloud"
-    />
-  </BehaviorTree>
-  <TreeNodesModel>
-    <SubTree ID="Scan Multiple Views">
-      <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
-        <Metadata runnable="true" />
-      </MetadataFields>
-    </SubTree>
-  </TreeNodesModel>
-</root>
diff --git a/src/lab_sim/objectives/scan_scene_-_multiple_point_clouds.xml b/src/lab_sim/objectives/scan_scene_-_multiple_point_clouds.xml
new file mode 100644
index 000000000..0f29542cc
--- /dev/null
+++ b/src/lab_sim/objectives/scan_scene_-_multiple_point_clouds.xml
@@ -0,0 +1,121 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root
+  BTCPP_format="4"
+  main_tree_to_execute="Scan Scene - Multiple Point Clouds"
+>
+  <!--//////////-->
+  <BehaviorTree
+    ID="Scan Scene - Multiple Point Clouds"
+    _description="Take multiple snapshots from the wrist camera at 3 positions (center, left, right) using Cartesian offsets and merge them into a single point cloud."
+    _favorite="true"
+  >
+    <Control ID="Sequence">
+      <!--Clear previous data-->
+      <SubTree ID="Clear Snapshot" _collapsed="true" />
+      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
+      <!--Move to center scan position and capture-->
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        joint_group_name="manipulator"
+        waypoint_name="Look at Table"
+      />
+      <SubTree
+        ID="Add Point Cloud to Vector"
+        _collapsed="true"
+        point_cloud_vector="{point_cloud_vector}"
+        topic_name="/wrist_camera/points"
+      />
+      <!--Cartesian move right from center and capture-->
+      <Action ID="ResetPoseStampedVector" vector="{cartesian_path}" />
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="grasp_link"
+        stamped_pose="{offset_pose}"
+        position_xyz="0.80;0;0"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        input="{offset_pose}"
+        vector="{cartesian_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="{cartesian_path}"
+        planning_group_name="manipulator"
+        position_only="false"
+        tip_links="grasp_link"
+        trajectory_sampling_rate="100"
+        velocity_scale_factor="1.000000"
+      />
+      <Action ID="ExecuteTrajectory" />
+      <SubTree
+        ID="Add Point Cloud to Vector"
+        _collapsed="true"
+        point_cloud_vector="{point_cloud_vector}"
+        topic_name="/wrist_camera/points"
+      />
+      <!--Cartesian move from right directly to left and capture-->
+      <Action ID="ResetPoseStampedVector" vector="{cartesian_path}" />
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="grasp_link"
+        stamped_pose="{offset_pose}"
+        position_xyz="-1.60;0;0"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        input="{offset_pose}"
+        vector="{cartesian_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="{cartesian_path}"
+        planning_group_name="manipulator"
+        position_only="false"
+        tip_links="grasp_link"
+        trajectory_sampling_rate="100"
+        velocity_scale_factor="1.000000"
+      />
+      <Action ID="ExecuteTrajectory" />
+      <SubTree
+        ID="Add Point Cloud to Vector"
+        _collapsed="true"
+        point_cloud_vector="{point_cloud_vector}"
+        topic_name="/wrist_camera/points"
+      />
+      <!--Merge all point clouds and display-->
+      <Action
+        ID="MergePointClouds"
+        grid_resolution_meters="0.010000"
+        merged_cloud="{merged_cloud}"
+        point_clouds="{point_cloud_vector}"
+      />
+      <Action
+        ID="SendPointCloudToUI"
+        point_cloud="{merged_cloud}"
+        pcd_topic="/pcd_pointcloud_captures"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Scan Scene - Multiple Point Clouds">
+      <MetadataFields>
+        <Metadata subcategory="Perception - 3D Point Cloud" />
+        <Metadata runnable="true" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/stack_blocks_with_icp.xml b/src/lab_sim/objectives/stack_blocks_with_icp.xml
index 0d7a55a53..0804e4c46 100644
--- a/src/lab_sim/objectives/stack_blocks_with_icp.xml
+++ b/src/lab_sim/objectives/stack_blocks_with_icp.xml
@@ -1,8 +1,8 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Stack Blocks with ICP">
+<root BTCPP_format="4" main_tree_to_execute="Stack Objects with ICP">
   <!--//////////-->
   <BehaviorTree
-    ID="Stack Blocks with ICP"
+    ID="Stack Objects with ICP"
     _description="Solution to the advanced exercise in the ICP section of the Perception &amp; ML tutorial"
     _favorite="false"
   >
@@ -29,7 +29,7 @@
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Stack Blocks with ICP">
+    <SubTree ID="Stack Objects with ICP">
       <MetadataFields>
         <Metadata runnable="true" />
         <Metadata subcategory="Training Examples" />
diff --git a/src/lab_sim/objectives/show_grasp_link.xml b/src/lab_sim/objectives/visualize_grasp_link.xml
similarity index 82%
rename from src/lab_sim/objectives/show_grasp_link.xml
rename to src/lab_sim/objectives/visualize_grasp_link.xml
index 97b1abb62..26ddff1cd 100644
--- a/src/lab_sim/objectives/show_grasp_link.xml
+++ b/src/lab_sim/objectives/visualize_grasp_link.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Show grasp_link">
+<root BTCPP_format="4" main_tree_to_execute="Visualize Grasp Link">
   <!--//////////-->
-  <BehaviorTree ID="Show grasp_link" _description="" _favorite="false">
+  <BehaviorTree ID="Visualize Grasp Link" _description="" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <SubTree
         ID="Get Transform Frame Pose"
@@ -26,7 +26,7 @@
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Show grasp_link">
+    <SubTree ID="Visualize Grasp Link">
       <MetadataFields>
         <Metadata runnable="true" />
         <Metadata subcategory="Application - Advanced Examples" />

From 971fd0833663ba4fe1c2620104a73b3b63c999f2 Mon Sep 17 00:00:00 2001
From: Dave Coleman <dave@picknik.ai>
Date: Fri, 6 Mar 2026 12:45:37 -0700
Subject: [PATCH 2/3] Fix waypoint name to match objective XML

Rename "Above Pick Cube" to "Above Pick Object" in ur_waypoints.yaml
to match the updated objective references.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
---
 src/lab_sim/objectives/move_along_square.xml | 2 +-
 src/lab_sim/waypoints/ur_waypoints.yaml      | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/lab_sim/objectives/move_along_square.xml b/src/lab_sim/objectives/move_along_square.xml
index c5292a813..6c8b72fbd 100644
--- a/src/lab_sim/objectives/move_along_square.xml
+++ b/src/lab_sim/objectives/move_along_square.xml
@@ -83,7 +83,7 @@
   <TreeNodesModel>
     <SubTree ID="Move Along Square">
       <MetadataFields>
-        <Metadata runnable="true" />
+        <Metadata runnable="false" />
         <Metadata subcategory="Application - Basic Examples" />
       </MetadataFields>
     </SubTree>
diff --git a/src/lab_sim/waypoints/ur_waypoints.yaml b/src/lab_sim/waypoints/ur_waypoints.yaml
index a74e2aa09..c03513913 100644
--- a/src/lab_sim/waypoints/ur_waypoints.yaml
+++ b/src/lab_sim/waypoints/ur_waypoints.yaml
@@ -40,7 +40,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Above Pick Cube
+  name: Above Pick Object
 - description: ''
   favorite: false
   joint_group_names:

From 8730d811bf76683744c9cb55ea813a11129fb4c6 Mon Sep 17 00:00:00 2001
From: Nathan Brooks <nathanbrooks@picknik.ai>
Date: Wed, 11 Mar 2026 15:25:27 -0600
Subject: [PATCH 3/3] Tune stall_velocity_threshold for lab_sim

---
 src/lab_sim/config/control/picknik_ur.ros2_control.yaml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/lab_sim/config/control/picknik_ur.ros2_control.yaml b/src/lab_sim/config/control/picknik_ur.ros2_control.yaml
index 022396adb..eaeae0477 100644
--- a/src/lab_sim/config/control/picknik_ur.ros2_control.yaml
+++ b/src/lab_sim/config/control/picknik_ur.ros2_control.yaml
@@ -92,6 +92,7 @@ robotiq_gripper_controller:
     joint: robotiq_85_left_knuckle_joint
     allow_stalling: true
     stall_timeout: 0.05
+    stall_velocity_threshold: 0.004
     goal_tolerance: 0.02
 
 force_torque_sensor_broadcaster: