diff --git a/.gitignore b/.gitignore
index ab974948..70b72a5e 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@ build/
 install/
 log/
 .vscode/
+MUJOCO_LOG.TXT
diff --git a/CLAUDE.md b/CLAUDE.md
new file mode 100644
index 00000000..c7dce189
--- /dev/null
+++ b/CLAUDE.md
@@ -0,0 +1,50 @@
+# AI Code Assistant Instructions for MoveIt Pro Example Workspace
+
+## MuJoCo Scene Files
+
+### Keyframe qpos must match model DOF count
+
+When editing `scene.xml` files (adding/removing bodies with joints), the `<keyframe>` section's `qpos` attribute must have exactly the number of values matching the model's total degrees of freedom. A mismatch causes `ros2_control_node` to crash with:
+
+```
+Error: keyframe 0: invalid qpos size, expected length <N>
+```
+
+Each joint type contributes to qpos:
+- **freejoint**: 7 values (x, y, z, qw, qx, qy, qz)
+- **hinge/slide**: 1 value each
+- **ball**: 4 values (quaternion)
+
+After adding or removing bodies with joints, either:
+1. **Remove the keyframe** and let MuJoCo use body `pos=` attributes for initial positions
+2. **Regenerate the keyframe** from the MuJoCo Interactive Viewer (Ctrl+Shift+K) once the scene is stable
+
+### MuJoCo documentation
+
+Refer to [docs.picknik.ai](https://docs.picknik.ai) for MuJoCo configuration guides:
+
+- [Physics Simulator Setup](https://docs.picknik.ai/how_to/configuration_tutorials/migrate_to_mujoco_config/) — creating scene.xml from URDF, camera/sensor setup, mesh conversion, MuJoCo Interactive Viewer
+- [config.yaml Reference](https://docs.picknik.ai/how_to/configuration_tutorials/config_yaml_reference/) — `hardware` section for `picknik_mujoco_ros/MujocoSystem` plugin configuration
+- [Simulator Keyframes Setup](https://docs.picknik.ai/how_to/configuration_tutorials/configure_keyframes/) — defining keyframes in scene.xml, `ResetMujocoKeyframe` Behavior
+- [Optimize Model Meshes](https://docs.picknik.ai/how_to/configuration_tutorials/optimizing_robot_model_meshes/) — MuJoCo enforces 1-200,000 faces per STL
+- [Simulation Troubleshooting](https://docs.picknik.ai/troubleshooting/Simulation%20Troubleshooting/) — physics parameters, grip stability, mass/inertia errors, rendering issues
+
+## Objective XML Files
+
+### MetadataFields required for CI
+
+Every objective XML file must include a `MetadataFields` block inside the `TreeNodesModel` section. The `validate_objectives` CI check will fail without it.
+
+```xml
+<TreeNodesModel>
+  <SubTree ID="My Objective Name">
+    <MetadataFields>
+      <Metadata runnable="true" />
+      <Metadata subcategory="Category Name" />
+    </MetadataFields>
+  </SubTree>
+</TreeNodesModel>
+```
+
+- `runnable` — set to `"true"` for top-level objectives the user can run, `"false"` for subtrees only called by other objectives
+- `subcategory` — groups the objective in the UI (e.g., `"AprilTag"`, `"Grasping"`, `"MuJoCo Simulation"`)
diff --git a/src/lab_sim/config/config.yaml b/src/lab_sim/config/config.yaml
index ca1ceeb2..e78f0500 100644
--- a/src/lab_sim/config/config.yaml
+++ b/src/lab_sim/config/config.yaml
@@ -48,6 +48,8 @@ objectives:
       - "moveit_pro::behaviors::VisionBehaviorsLoader"
       - "moveit_pro::behaviors::ConverterBehaviorsLoader"
       - "moveit_pro::behaviors::MujocoBehaviorsLoader"
+    lab_sim:
+      - "lab_sim_behaviors::LabSimBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:
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 022396ad..4638d966 100644
--- a/src/lab_sim/config/control/picknik_ur.ros2_control.yaml
+++ b/src/lab_sim/config/control/picknik_ur.ros2_control.yaml
@@ -64,18 +64,25 @@ joint_trajectory_controller:
       stopped_velocity_tolerance: 0.0
       goal_time: 0.0
       linear_rail_joint:
+        trajectory: 0.5
         goal: 0.05
       shoulder_pan_joint:
+        trajectory: 0.5
         goal: 0.05
       shoulder_lift_joint:
+        trajectory: 0.5
         goal: 0.05
       elbow_joint:
+        trajectory: 0.5
         goal: 0.05
       wrist_1_joint:
+        trajectory: 0.5
         goal: 0.05
       wrist_2_joint:
+        trajectory: 0.5
         goal: 0.05
       wrist_3_joint:
+        trajectory: 0.5
         goal: 0.05
     acceleration_limits:
       linear_rail_joint: 10.0
@@ -91,8 +98,8 @@ robotiq_gripper_controller:
     default: true
     joint: robotiq_85_left_knuckle_joint
     allow_stalling: true
-    stall_timeout: 0.05
-    goal_tolerance: 0.02
+    stall_timeout: 0.3
+    goal_tolerance: 0.3
 
 force_torque_sensor_broadcaster:
   ros__parameters:
diff --git a/src/lab_sim/description/keyboard-texture.png b/src/lab_sim/description/keyboard-texture.png
new file mode 100644
index 00000000..9e685eab
--- /dev/null
+++ b/src/lab_sim/description/keyboard-texture.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f0b212c56658ff5e5936f82867413fb357322e432664609c6b496c190925d20c
+size 170370
diff --git a/src/lab_sim/description/laptop-screen.png b/src/lab_sim/description/laptop-screen.png
new file mode 100644
index 00000000..67982b9f
--- /dev/null
+++ b/src/lab_sim/description/laptop-screen.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4045f46ccec667d8c317cde63758d9ab826f8990ebcdee163ca10a0241d52b09
+size 215006
diff --git a/src/lab_sim/description/picknik_ur.xacro b/src/lab_sim/description/picknik_ur.xacro
index fe73e3e8..98d1a434 100644
--- a/src/lab_sim/description/picknik_ur.xacro
+++ b/src/lab_sim/description/picknik_ur.xacro
@@ -663,7 +663,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0" />
       <geometry>
-        <box size="5.0 0.6 0.30" />
+        <box size="5.0 0.2 0.30" />
       </geometry>
       <material name="grey">
         <color rgba="0.1 0.1 0.1 1.0" />
@@ -672,7 +672,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0.14 0.125" />
       <geometry>
-        <box size="5.0 0.2 0.5" />
+        <box size="5.0 0.12 0.5" />
       </geometry>
       <material name="grey">
         <color rgba="0.1 0.1 0.1 1.0" />
@@ -681,7 +681,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 -0.22 0.125" />
       <geometry>
-        <box size="5.0 0.12 0.5" />
+        <box size="5.0 0.08 0.5" />
       </geometry>
       <material name="grey">
         <color rgba="0.1 0.1 0.1 1.0" />
@@ -690,7 +690,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0" />
       <geometry>
-        <box size="5.0 0.6 0.3" />
+        <box size="5.0 0.2 0.3" />
       </geometry>
       <material name="grey">
         <color rgba="0.1 0.1 0.1 1.0" />
@@ -699,7 +699,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0.14 0.125" />
       <geometry>
-        <box size="5.0 0.2 0.5" />
+        <box size="5.0 0.12 0.5" />
       </geometry>
       <material name="grey">
         <color rgba="0.1 0.1 0.1 1.0" />
@@ -708,7 +708,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 -0.22 0.125" />
       <geometry>
-        <box size="5.0 0.12 0.5" />
+        <box size="5.0 0.08 0.5" />
       </geometry>
       <material name="grey">
         <color rgba="0.1 0.1 0.1 1.0" />
diff --git a/src/lab_sim/description/scene.xml b/src/lab_sim/description/scene.xml
index 7d896c29..18e75e90 100644
--- a/src/lab_sim/description/scene.xml
+++ b/src/lab_sim/description/scene.xml
@@ -6,12 +6,87 @@
 
   <compiler angle="radian" autolimits="true" />
 
-  <option integrator="implicitfast" />
+  <option integrator="implicitfast" impratio="10" noslip_iterations="3" />
+
+  <!-- nuser_cam=1 enables per-camera user params: 0=RGB+Depth, 1=RGB-only, 2=3D-lidar -->
+  <size nuser_cam="1" />
 
   <asset>
-    <mesh name="cube" file="assets/Cube.stl" />
+    <!-- Laptop keyboard texture (dark checker pattern) -->
+    <texture
+      name="keyboard_tex"
+      type="2d"
+      builtin="checker"
+      rgb1="0.03 0.03 0.03"
+      rgb2="0.08 0.08 0.08"
+      width="256"
+      height="256"
+    />
+    <material
+      name="keyboard_mat"
+      texture="keyboard_tex"
+      texrepeat="10 6"
+      specular="0.2"
+      shininess="0.3"
+    />
+    <!-- Laptop screen texture (dark IDE-like gradient) -->
+    <texture
+      name="screen_tex"
+      type="2d"
+      builtin="gradient"
+      rgb1="0.05 0.12 0.18"
+      rgb2="0.02 0.06 0.10"
+      width="256"
+      height="256"
+    />
+    <material name="screen_mat" texture="screen_tex" emission="0.4" />
+    <texture
+      name="desktop_screenshot_tex"
+      type="cube"
+      filedown="laptop-screen.png"
+      rgb1="0.02 0.02 0.02"
+    />
+    <material
+      name="desktop_screenshot_mat"
+      texture="desktop_screenshot_tex"
+      emission="0.3"
+    />
+    <!-- Keyboard texture -->
+    <texture
+      name="keyboard_img_tex"
+      type="cube"
+      filefront="keyboard-texture.png"
+      rgb1="0.15 0.15 0.15"
+    />
+    <material
+      name="keyboard_img_mat"
+      texture="keyboard_img_tex"
+      specular="0.2"
+      shininess="0.3"
+    />
+    <!-- AprilTag material for tray is provided by desk_globals.xml (apriltag_material1) -->
+    <!-- Red plastic tray material -->
+    <material
+      name="red_plastic"
+      rgba="0.75 0.15 0.1 1"
+      specular="0.6"
+      shininess="0.7"
+    />
+    <!-- Light brown plastic pot material -->
+    <material
+      name="pot_plastic"
+      rgba="0.72 0.52 0.32 1"
+      specular="0.5"
+      shininess="0.6"
+    />
   </asset>
 
+  <!-- Set offscreen rendering resolution to match camera sensor resolution (640x480) so that
+       MuJoCo renders images at the correct size for the simulated wrist and scene cameras. -->
+  <visual>
+    <global offwidth="640" offheight="480" />
+  </visual>
+
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
 
@@ -31,51 +106,960 @@
     <!-- Scene Camera -->
     <site
       name="scene_camera_optical_frame"
-      pos="-1.0 -2.0 2.966"
-      euler="3.99 0.000 0"
+      pos="1.8 -0.495 1.0"
+      xyaxes="0.586 0.810 0.000 -0.186 0.135 0.973"
     />
     <camera
       name="scene_camera"
-      pos="-1.0 -2.0 2.966"
+      pos="1.8 -0.495 1.0"
       fovy="58"
       mode="fixed"
       resolution="640 480"
-      euler="0.849 0.000 0.0"
+      xyaxes="0.586 0.810 0.000 -0.186 0.135 0.973"
+      user="1"
     />
     <!-- lab desk -->
     <include file="lab_desk/desk.xml" />
-    <!-- Define the block with AprilTags on each face -->
-    <body name="block1" pos="0.2 0.75 0.575">
-      <freejoint name="block1" />
+    <!-- Modern laptop (white Mac) -->
+    <body name="laptop" pos="-0.6 0.6718 0.474" euler="0 0 0.2618">
+      <freejoint />
+      <!-- Base / keyboard deck -->
       <geom
+        name="laptop_base"
         type="box"
-        size="0.025 0.025 0.025"
-        rgba="1 1 1 1"
-        material="apriltag_material1"
+        size="0.15 0.10 0.005"
+        pos="0 0 0.005"
+        material="keyboard_img_mat"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Screen - tilted back ~20 degrees past vertical, pushed forward past bezel -->
+      <geom
+        name="laptop_screen"
+        type="box"
+        size="0.15 0.003 0.10"
+        pos="0 0.138 0.104"
+        euler="-0.35 0 0"
+        material="desktop_screenshot_mat"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Collision: base -->
+      <geom
+        name="laptop_base_col"
+        type="box"
+        size="0.15 0.10 0.005"
+        pos="0 0 0.005"
+        group="4"
+      />
+      <!-- Collision: screen -->
+      <geom
+        name="laptop_screen_col"
+        type="box"
+        size="0.155 0.004 0.105"
+        pos="0 0.133 0.104"
+        euler="-0.35 0 0"
+        group="4"
+      />
+    </body>
+    <!-- Fake potted plant (left of microscope) -->
+    <body name="fake_plant" pos="-1.55 0.5768 0.474">
+      <freejoint />
+      <!-- Pot -->
+      <geom
+        name="plant_pot"
+        type="cylinder"
+        size="0.03 0.03"
+        pos="0 0 0.03"
+        material="pot_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="plant_pot_rim"
+        type="cylinder"
+        size="0.033 0.004"
+        pos="0 0 0.056"
+        material="pot_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Thin stem -->
+      <geom
+        name="plant_stem"
+        type="cylinder"
+        size="0.004 0.06"
+        pos="0 0 0.12"
+        rgba="0.3 0.25 0.15 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Large foliage puff -->
+      <geom
+        name="plant_foliage_1"
+        type="sphere"
+        size="0.06"
+        pos="0 0 0.2"
+        rgba="0.15 0.45 0.15 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="plant_foliage_2"
+        type="sphere"
+        size="0.045"
+        pos="0.035 0.02 0.22"
+        rgba="0.2 0.5 0.18 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="plant_foliage_3"
+        type="sphere"
+        size="0.04"
+        pos="-0.03 0.025 0.23"
+        rgba="0.12 0.42 0.12 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="plant_foliage_4"
+        type="sphere"
+        size="0.04"
+        pos="0.01 -0.035 0.22"
+        rgba="0.18 0.48 0.15 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="plant_foliage_5"
+        type="sphere"
+        size="0.035"
+        pos="-0.02 -0.02 0.25"
+        rgba="0.16 0.46 0.14 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Collision: pot -->
+      <geom
+        name="plant_pot_col"
+        type="cylinder"
+        size="0.033 0.032"
+        pos="0 0 0.032"
+        group="4"
+      />
+    </body>
+    <!-- Bronze alligator statue (top shelf) -->
+    <body name="alligator_statue" pos="-0.5 0.8768 1.365">
+      <freejoint />
+      <!-- Trophy base - dark wood with gold nameplate -->
+      <geom
+        name="gator_base"
+        type="box"
+        size="0.09 0.045 0.012"
+        pos="-0.01 0 -0.030"
+        rgba="0.12 0.06 0.02 1"
+      />
+      <geom
+        name="gator_base_top"
+        type="box"
+        size="0.08 0.04 0.003"
+        pos="-0.01 0 -0.015"
+        rgba="0.15 0.08 0.03 1"
+      />
+      <geom
+        name="gator_nameplate"
+        type="box"
+        size="0.03 0.001 0.008"
+        pos="-0.01 -0.041 -0.030"
+        rgba="0.75 0.65 0.25 1"
+      />
+      <!-- Body - three segments for organic shape -->
+      <geom
+        name="gator_body_front"
+        type="capsule"
+        size="0.013 0.02"
+        pos="0.02 0 0"
+        euler="0 1.5708 0"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_body_mid"
+        type="capsule"
+        size="0.014 0.025"
+        pos="-0.015 0 0.001"
+        euler="0 1.5708 0"
+        rgba="0.18 0.65 0.18 1"
+      />
+      <geom
+        name="gator_body_rear"
+        type="capsule"
+        size="0.012 0.02"
+        pos="-0.045 0 0"
+        euler="0 1.5708 0"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Head - wider, flatter -->
+      <geom
+        name="gator_head"
+        type="box"
+        size="0.022 0.012 0.007"
+        pos="0.06 0 -0.001"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Upper jaw -->
+      <geom
+        name="gator_upper_jaw"
+        type="box"
+        size="0.02 0.008 0.004"
+        pos="0.09 0 0.001"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Lower jaw -->
+      <geom
+        name="gator_lower_jaw"
+        type="box"
+        size="0.018 0.007 0.003"
+        pos="0.088 0 -0.005"
+        rgba="0.3 0.75 0.25 1"
+      />
+      <!-- Snout tip -->
+      <geom
+        name="gator_snout"
+        type="capsule"
+        size="0.005 0.008"
+        pos="0.108 0 -0.001"
+        euler="0 1.5708 0"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Eye ridges -->
+      <geom
+        name="gator_eye_l"
+        type="sphere"
+        size="0.004"
+        pos="0.055 0.01 0.007"
+        rgba="0.9 0.85 0.1 1"
+      />
+      <geom
+        name="gator_eye_r"
+        type="sphere"
+        size="0.004"
+        pos="0.055 -0.01 0.007"
+        rgba="0.9 0.85 0.1 1"
+      />
+      <!-- Dorsal ridges -->
+      <geom
+        name="gator_ridge_1"
+        type="sphere"
+        size="0.004"
+        pos="0.03 0 0.013"
+        rgba="0.15 0.55 0.12 1"
+      />
+      <geom
+        name="gator_ridge_2"
+        type="sphere"
+        size="0.004"
+        pos="0.01 0 0.014"
+        rgba="0.15 0.55 0.12 1"
+      />
+      <geom
+        name="gator_ridge_3"
+        type="sphere"
+        size="0.004"
+        pos="-0.01 0 0.014"
+        rgba="0.15 0.55 0.12 1"
+      />
+      <geom
+        name="gator_ridge_4"
+        type="sphere"
+        size="0.003"
+        pos="-0.03 0 0.012"
+        rgba="0.15 0.55 0.12 1"
+      />
+      <geom
+        name="gator_ridge_5"
+        type="sphere"
+        size="0.003"
+        pos="-0.045 0 0.011"
+        rgba="0.15 0.55 0.12 1"
+      />
+      <!-- Tail - three tapering segments with slight curve -->
+      <geom
+        name="gator_tail_1"
+        type="capsule"
+        size="0.009 0.02"
+        pos="-0.08 0 0.002"
+        euler="0 1.6 0"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_tail_2"
+        type="capsule"
+        size="0.006 0.02"
+        pos="-0.115 0 0.006"
+        euler="0 1.7 0.1"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_tail_3"
+        type="capsule"
+        size="0.003 0.015"
+        pos="-0.145 0.005 0.009"
+        euler="0 1.75 0.2"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Front legs with feet -->
+      <geom
+        name="gator_leg_fl"
+        type="capsule"
+        size="0.005 0.01"
+        pos="0.025 0.018 -0.008"
+        euler="0.5 0 0.2"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_foot_fl"
+        type="box"
+        size="0.006 0.005 0.002"
+        pos="0.025 0.028 -0.016"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_leg_fr"
+        type="capsule"
+        size="0.005 0.01"
+        pos="0.025 -0.018 -0.008"
+        euler="-0.5 0 -0.2"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_foot_fr"
+        type="box"
+        size="0.006 0.005 0.002"
+        pos="0.025 -0.028 -0.016"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Rear legs with feet (slightly larger) -->
+      <geom
+        name="gator_leg_bl"
+        type="capsule"
+        size="0.005 0.012"
+        pos="-0.04 0.018 -0.008"
+        euler="0.5 0 0.2"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_foot_bl"
+        type="box"
+        size="0.007 0.005 0.002"
+        pos="-0.04 0.030 -0.018"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_leg_br"
+        type="capsule"
+        size="0.005 0.012"
+        pos="-0.04 -0.018 -0.008"
+        euler="-0.5 0 -0.2"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_foot_br"
+        type="box"
+        size="0.007 0.005 0.002"
+        pos="-0.04 -0.030 -0.018"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Collision: simplified bounding box for entire statue -->
+      <geom
+        name="gator_col"
+        type="box"
+        size="0.16 0.035 0.025"
+        pos="-0.01 0 -0.005"
+        group="4"
       />
     </body>
-    <body name="block2" pos="0.0 0.75 0.575">
-      <freejoint name="block2" />
-      <geom class="visual" mesh="cube" pos="0 0 -0.03" />
-      <geom class="collision" type="box" size="0.025 0.025 0.025" pos="0 0 0" />
+    <!-- Test tube rack -->
+    <body name="test_tube_rack" pos="0.3 0.5468 0.474">
+      <freejoint />
+      <!-- Base plate -->
+      <geom
+        name="rack_base"
+        type="box"
+        size="0.065 0.025 0.003"
+        pos="0 0 0.003"
+        rgba="0.6 0.6 0.65 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Front wall -->
+      <geom
+        name="rack_front"
+        type="box"
+        size="0.065 0.003 0.025"
+        pos="0 -0.022 0.031"
+        rgba="0.6 0.6 0.65 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Back wall -->
+      <geom
+        name="rack_back"
+        type="box"
+        size="0.065 0.003 0.025"
+        pos="0 0.022 0.031"
+        rgba="0.6 0.6 0.65 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Left end -->
+      <geom
+        name="rack_left"
+        type="box"
+        size="0.003 0.025 0.025"
+        pos="-0.062 0 0.031"
+        rgba="0.6 0.6 0.65 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Right end -->
+      <geom
+        name="rack_right"
+        type="box"
+        size="0.003 0.025 0.025"
+        pos="0.062 0 0.031"
+        rgba="0.6 0.6 0.65 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Collision: overall rack box (heavy to keep tubes upright) -->
+      <geom
+        name="rack_col"
+        type="box"
+        size="0.065 0.025 0.028"
+        pos="0 0 0.028"
+        group="4"
+        mass="50.0"
+      />
     </body>
-    <body name="block3" pos="-0.2 0.75 0.575">
-      <freejoint name="block3" />
+    <!-- Red plastic sorting tray -->
+    <body name="sorting_tray" pos="0.95 0.5568 0.474" euler="0 0 0.0873">
+      <freejoint />
+      <!-- Base plate -->
       <geom
+        name="tray_base"
         type="box"
-        size="0.025 0.025 0.025"
-        rgba="1 1 1 1"
+        size="0.1 0.2 0.003"
+        pos="0 0 0.003"
+        material="red_plastic"
+        mass="0.5"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Front lip -->
+      <geom
+        name="tray_front"
+        type="box"
+        size="0.097 0.003 0.013"
+        pos="0 -0.2 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Back lip -->
+      <geom
+        name="tray_back"
+        type="box"
+        size="0.097 0.003 0.013"
+        pos="0 0.2 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Left lip -->
+      <geom
+        name="tray_left"
+        type="box"
+        size="0.003 0.197 0.013"
+        pos="-0.1 0 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Right lip -->
+      <geom
+        name="tray_right"
+        type="box"
+        size="0.003 0.197 0.013"
+        pos="0.1 0 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Rounded corners -->
+      <geom
+        name="tray_corner_fl"
+        type="cylinder"
+        size="0.003 0.013"
+        pos="-0.1 -0.2 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tray_corner_fr"
+        type="cylinder"
+        size="0.003 0.013"
+        pos="0.1 -0.2 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tray_corner_bl"
+        type="cylinder"
+        size="0.003 0.013"
+        pos="-0.1 0.2 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tray_corner_br"
+        type="cylinder"
+        size="0.003 0.013"
+        pos="0.1 0.2 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- AprilTag on tray floor -->
+      <geom
+        name="tray_apriltag"
+        type="box"
+        size="0.04 0.04 0.0005"
+        pos="0 -0.15 0.0065"
         material="apriltag_material2"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Collision: tray base + lips as one box -->
+      <geom
+        name="tray_base_col"
+        type="box"
+        size="0.103 0.203 0.003"
+        pos="0 0 0.003"
+        group="4"
+        mass="0.5"
+        friction="2.0 0.1 0.01"
+        condim="4"
+      />
+      <geom
+        name="tray_front_col"
+        type="box"
+        size="0.103 0.003 0.016"
+        pos="0 -0.203 0.016"
+        group="4"
+        friction="2.0 0.1 0.01"
+        condim="4"
+      />
+      <geom
+        name="tray_back_col"
+        type="box"
+        size="0.103 0.003 0.016"
+        pos="0 0.203 0.016"
+        group="4"
+        friction="2.0 0.1 0.01"
+        condim="4"
+      />
+      <geom
+        name="tray_left_col"
+        type="box"
+        size="0.003 0.203 0.016"
+        pos="-0.103 0 0.016"
+        group="4"
+        friction="2.0 0.1 0.01"
+        condim="4"
+      />
+      <geom
+        name="tray_right_col"
+        type="box"
+        size="0.003 0.203 0.016"
+        pos="0.103 0 0.016"
+        group="4"
+        friction="2.0 0.1 0.01"
+        condim="4"
+      />
+    </body>
+    <!-- Test tubes with colored liquids (manipulable) -->
+    <body name="tube_red" pos="0.25 0.5468 0.532">
+      <freejoint name="tube_red" />
+      <geom
+        name="tube_red_glass"
+        type="capsule"
+        size="0.0065 0.045"
+        rgba="0.85 0.88 0.9 0.3"
+        mass="0.01"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tube_red_liquid"
+        type="capsule"
+        size="0.005 0.03"
+        pos="0 0 -0.008"
+        rgba="0.9 0.1 0.1 0.85"
+        contype="0"
+        conaffinity="0"
+        mass="0.03"
+      />
+      <geom name="tube_red_col" type="capsule" size="0.0065 0.045" group="4" />
+    </body>
+    <body name="tube_blue" pos="0.27 0.5468 0.532">
+      <freejoint name="tube_blue" />
+      <geom
+        name="tube_blue_glass"
+        type="capsule"
+        size="0.0065 0.045"
+        rgba="0.85 0.88 0.9 0.3"
+        mass="0.01"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tube_blue_liquid"
+        type="capsule"
+        size="0.005 0.03"
+        pos="0 0 -0.008"
+        rgba="0.1 0.2 0.9 0.85"
+        contype="0"
+        conaffinity="0"
+        mass="0.03"
+      />
+      <geom name="tube_blue_col" type="capsule" size="0.0065 0.045" group="4" />
+    </body>
+    <body name="tube_green" pos="0.29 0.5468 0.532">
+      <freejoint name="tube_green" />
+      <geom
+        name="tube_green_glass"
+        type="capsule"
+        size="0.0065 0.045"
+        rgba="0.85 0.88 0.9 0.3"
+        mass="0.01"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tube_green_liquid"
+        type="capsule"
+        size="0.005 0.03"
+        pos="0 0 -0.008"
+        rgba="0.1 0.8 0.2 0.85"
+        contype="0"
+        conaffinity="0"
+        mass="0.03"
+      />
+      <geom
+        name="tube_green_col"
+        type="capsule"
+        size="0.0065 0.045"
+        group="4"
+      />
+    </body>
+    <body name="tube_yellow" pos="0.31 0.5468 0.532">
+      <freejoint name="tube_yellow" />
+      <geom
+        name="tube_yellow_glass"
+        type="capsule"
+        size="0.0065 0.045"
+        rgba="0.85 0.88 0.9 0.3"
+        mass="0.01"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tube_yellow_liquid"
+        type="capsule"
+        size="0.005 0.03"
+        pos="0 0 -0.008"
+        rgba="0.95 0.9 0.1 0.85"
+        contype="0"
+        conaffinity="0"
+        mass="0.03"
+      />
+      <geom
+        name="tube_yellow_col"
+        type="capsule"
+        size="0.0065 0.045"
+        group="4"
+      />
+    </body>
+    <body name="tube_purple" pos="0.33 0.5468 0.532">
+      <freejoint name="tube_purple" />
+      <geom
+        name="tube_purple_glass"
+        type="capsule"
+        size="0.0065 0.045"
+        rgba="0.85 0.88 0.9 0.3"
+        mass="0.01"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tube_purple_liquid"
+        type="capsule"
+        size="0.005 0.03"
+        pos="0 0 -0.008"
+        rgba="0.6 0.1 0.8 0.85"
+        contype="0"
+        conaffinity="0"
+        mass="0.03"
+      />
+      <geom
+        name="tube_purple_col"
+        type="capsule"
+        size="0.0065 0.045"
+        group="4"
+      />
+    </body>
+    <body name="tube_orange" pos="0.35 0.5468 0.532">
+      <freejoint name="tube_orange" />
+      <geom
+        name="tube_orange_glass"
+        type="capsule"
+        size="0.0065 0.045"
+        rgba="0.85 0.88 0.9 0.3"
+        mass="0.01"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tube_orange_liquid"
+        type="capsule"
+        size="0.005 0.03"
+        pos="0 0 -0.008"
+        rgba="0.95 0.5 0.05 0.85"
+        contype="0"
+        conaffinity="0"
+        mass="0.03"
+      />
+      <geom
+        name="tube_orange_col"
+        type="capsule"
+        size="0.0065 0.045"
+        group="4"
+      />
+    </body>
+    <!-- Medicine bottles (manipulable) -->
+    <body name="bottle_white" pos="-0.16 0.5068 0.519">
+      <freejoint name="bottle_white" />
+      <geom
+        name="bottle_white_body"
+        type="cylinder"
+        size="0.02 0.045"
+        rgba="0.92 0.92 0.92 1"
+        mass="0.08"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_white_cap"
+        type="cylinder"
+        size="0.021 0.008"
+        pos="0 0 0.053"
+        rgba="0.1 0.5 0.1 1"
+        mass="0.02"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- AprilTag on cap (visible from above) -->
+      <geom
+        name="bottle_white_apriltag"
+        type="box"
+        size="0.018 0.018 0.003"
+        pos="0 0 0.065"
+        material="apriltag_material1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_white_col"
+        type="cylinder"
+        size="0.021 0.053"
+        group="4"
+        friction="1.5 0.01 0.001"
+        condim="4"
+      />
+    </body>
+    <body name="bottle_amber" pos="0.07 0.4868 0.519">
+      <freejoint name="bottle_amber" />
+      <geom
+        name="bottle_amber_body"
+        type="cylinder"
+        size="0.02 0.045"
+        rgba="0.55 0.33 0.08 0.75"
+        mass="0.08"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_amber_cap"
+        type="cylinder"
+        size="0.021 0.008"
+        pos="0 0 0.053"
+        rgba="0.9 0.9 0.9 1"
+        mass="0.02"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- AprilTag on white cap (visible from above) -->
+      <geom
+        name="bottle_amber_apriltag"
+        type="box"
+        size="0.018 0.018 0.003"
+        pos="0 0 0.065"
+        material="apriltag_material1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_amber_col"
+        type="cylinder"
+        size="0.021 0.053"
+        group="4"
+        friction="1.5 0.01 0.001"
+        condim="4"
+      />
+    </body>
+    <body name="bottle_blue" pos="-0.04 0.5768 0.519">
+      <freejoint name="bottle_blue" />
+      <geom
+        name="bottle_blue_body"
+        type="cylinder"
+        size="0.02 0.045"
+        rgba="0.15 0.25 0.65 0.85"
+        mass="0.08"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_blue_cap"
+        type="cylinder"
+        size="0.021 0.008"
+        pos="0 0 0.053"
+        rgba="0.1 0.2 0.8 1"
+        mass="0.02"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- AprilTag on cap (visible from above) -->
+      <geom
+        name="bottle_blue_apriltag"
+        type="box"
+        size="0.018 0.018 0.003"
+        pos="0 0 0.065"
+        material="apriltag_material1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_blue_col"
+        type="cylinder"
+        size="0.021 0.053"
+        group="4"
+        friction="1.5 0.01 0.001"
+        condim="4"
+      />
+    </body>
+    <body name="bottle_green" pos="0.15 0.5968 0.519">
+      <freejoint name="bottle_green" />
+      <geom
+        name="bottle_green_body"
+        type="cylinder"
+        size="0.02 0.045"
+        rgba="0.15 0.5 0.25 0.85"
+        mass="0.08"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_green_cap"
+        type="cylinder"
+        size="0.021 0.008"
+        pos="0 0 0.053"
+        rgba="0.8 0.1 0.1 1"
+        mass="0.02"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- AprilTag on cap (visible from above) -->
+      <geom
+        name="bottle_green_apriltag"
+        type="box"
+        size="0.018 0.018 0.003"
+        pos="0 0 0.065"
+        material="apriltag_material1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_green_col"
+        type="cylinder"
+        size="0.021 0.053"
+        group="4"
+        friction="1.5 0.01 0.001"
+        condim="4"
+      />
+    </body>
+    <body name="bottle_red" pos="-0.13 0.6268 0.519">
+      <freejoint name="bottle_red" />
+      <geom
+        name="bottle_red_body"
+        type="cylinder"
+        size="0.02 0.045"
+        rgba="0.7 0.15 0.1 0.85"
+        mass="0.08"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_red_cap"
+        type="cylinder"
+        size="0.021 0.008"
+        pos="0 0 0.053"
+        rgba="0.9 0.9 0.9 1"
+        mass="0.02"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- AprilTag on white cap (visible from above) -->
+      <geom
+        name="bottle_red_apriltag"
+        type="box"
+        size="0.018 0.018 0.003"
+        pos="0 0 0.065"
+        material="apriltag_material1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_red_col"
+        type="cylinder"
+        size="0.021 0.053"
+        group="4"
+        friction="1.5 0.01 0.001"
+        condim="4"
       />
     </body>
+    <!-- Removed bottle_purple and bottle_yellow to reduce crowding -->
     <include file="ur5e/ur5e_linear_rail.xml" />
   </worldbody>
 
   <keyframe>
     <key
       name="default"
-      qpos="-1.168 0.776711 0.469754 1 5.72696e-06 -4.26533e-08 -9.38821e-05 -1.34591 1.07777 0.902167 1 -2.70778e-08 7.60222e-07 -8.18971e-05 -1.1503 1.07777 0.902163 -0.873311 -1.32611e-06 -2.31428e-06 -0.487163 -0.871046 0.729603 0.473006 -0.839083 -3.76738e-06 -5.23752e-06 -0.544003 -0.699998 0.85 0.472753 -0.759699 6.25044e-07 1.49157e-06 -0.650275 -0.5 0.69 0.472779 -0.75966 -8.69884e-06 3.09906e-06 -0.65032 -0.360001 0.609999 0.472778 -0.759663 -6.29279e-06 3.61581e-06 -0.650317 -0.299999 0.7 0.472779 -0.75966 -5.34077e-06 1.33814e-06 -0.65032 -3.70361e-09 0.9 0.47375 -0.839072 -1.82055e-05 -5.31435e-05 0.544021 -0.1 0.85 0.47375 0.283662 4.83891e-05 2.85336e-05 -0.958924 0.0952783 0.840677 0.47375 -0.852936 4.00776e-05 -3.93632e-05 -0.522015 0.2 0.75 0.498669 1 3.49711e-17 2.9976e-18 1.51884e-30 1.73912e-05 0.749992 0.498669 1 0.00016562 0.000360588 4.6626e-11 -0.2 0.75 0.498669 1 2.94315e-17 -7.92697e-18 -4.14434e-30 1.56909e-06 -3.71262e-07 -1.88491 1.10086 -1.25647 -1.57075 -3.63345e-07 0.0525545 0.000140183 0.0525603 -0.050625 0.0525545 0.000140184 0.0525567 -0.0506181"
-      qvel="0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0"
+      qpos="-1.168 0.573511 0.469754 1 5.72696e-06 -4.26533e-08 -9.38821e-05 -1.34591 0.87457 0.902167 1 -2.70778e-08 7.60222e-07 -8.18971e-05 -1.1503 0.87457 0.902163 -0.873311 -1.32611e-06 -2.31428e-06 -0.487163 -0.871046 0.526403 0.473006 -0.839083 -3.76738e-06 -5.23752e-06 -0.544003 0.605 0.5578 0.473827 1 0 0 0 0.3 0.6468 0.472753 1 0 0 0 -0.5 0.4868 0.472779 -0.75966 -8.69884e-06 3.09906e-06 -0.65032 -0.360001 0.406799 0.472778 -0.759663 -6.29279e-06 3.61581e-06 -0.650317 -0.299999 0.4968 0.472779 -0.75966 -5.34077e-06 1.33814e-06 -0.65032 0.15 0.3468 0.47375 -0.839072 -1.82055e-05 -5.31435e-05 0.544021 0.12 0.4468 0.47375 0.283662 4.83891e-05 2.85336e-05 -0.958924 0.22 0.3968 0.47375 -0.852936 4.00776e-05 -3.93632e-05 -0.522015 -0.6 0.6718 0.474 0.9914 0 0 0.1305 -1.55 0.5768 0.474 1 0 0 0 -0.5 0.8768 1.365 1 0 0 0 0.3 0.5468 0.474 1 0 0 0 0.95 0.5568 0.474 0.999 0 0 0.0436 0.25 0.5468 0.532 1 0 0 0 0.27 0.5468 0.532 1 0 0 0 0.29 0.5468 0.532 1 0 0 0 0.31 0.5468 0.532 1 0 0 0 0.33 0.5468 0.532 1 0 0 0 0.35 0.5468 0.532 1 0 0 0 -0.16 0.5068 0.519 0.9945 0 0 0.1045 0.07 0.4868 0.519 0.9976 0 0 -0.0698 -0.04 0.5768 0.519 0.9990 0 0 0.0436 0.15 0.5968 0.519 0.9914 0 0 -0.1305 -0.13 0.6268 0.519 0.9848 0 0 0.1736 1.56909e-06 -3.71262e-07 -1.88491 1.10086 -1.25647 -1.57075 -3.63345e-07 0.0525545 0.000140183 0.0525603 -0.050625 0.0525545 0.000140184 0.0525567 -0.0506181"
+      qvel="0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0"
       ctrl="0 -1.8849 1.0997 -1.2566 -1.57075 0 0 0"
     />
   </keyframe>
diff --git a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
index c0f1f975..c4698d04 100644
--- a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
+++ b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
@@ -10,7 +10,7 @@
       <!--Setup the environment to its "home" configuration - move to center and open gripper-->
       <SubTree
         ID="Move to Waypoint"
-        waypoint_name="Look at Table"
+        waypoint_name="3 Waypoint Mid Point"
         joint_group_name="manipulator"
       />
       <Action
@@ -26,7 +26,7 @@
           <Control ID="Sequence">
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Pick Cube"
+              waypoint_name="3 Waypoint Pick Pose"
               joint_group_name="manipulator"
             />
             <Action
@@ -37,13 +37,13 @@
             />
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Look at Table"
+              waypoint_name="3 Waypoint Mid Point"
               joint_group_name="manipulator"
             />
             <SubTree
               ID="Move to Waypoint"
               joint_group_name="manipulator"
-              waypoint_name="Place Cube"
+              waypoint_name="3 Waypoint Place Pose"
             />
             <Action
               ID="MoveGripperAction"
@@ -53,7 +53,7 @@
             />
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Look at Table"
+              waypoint_name="3 Waypoint Mid Point"
               joint_group_name="manipulator"
             />
           </Control>
@@ -61,7 +61,7 @@
           <Control ID="Sequence">
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Place Cube"
+              waypoint_name="3 Waypoint Place Pose"
               joint_group_name="manipulator"
             />
             <Action
@@ -72,12 +72,12 @@
             />
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Look at Table"
+              waypoint_name="3 Waypoint Mid Point"
               joint_group_name="manipulator"
             />
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Pick Cube"
+              waypoint_name="3 Waypoint Pick Pose"
               joint_group_name="manipulator"
             />
             <Action
@@ -88,7 +88,7 @@
             />
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Look at Table"
+              waypoint_name="3 Waypoint Mid Point"
               joint_group_name="manipulator"
             />
           </Control>
diff --git a/src/lab_sim/objectives/_scan_scene.xml b/src/lab_sim/objectives/_scan_scene.xml
deleted file mode 100644
index 334a261c..00000000
--- 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 6960f3d7..6e0a3e6d 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 a231afb7..14ee2e03 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/constrained_pick_and_place_subtree.xml b/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml
index 28df92cd..2cc26bfc 100644
--- a/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml
+++ b/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version="1.0" encoding="UTF-8" ?>
 <root
   BTCPP_format="4"
   main_tree_to_execute="Constrained Pick and Place Subtree"
@@ -6,81 +6,278 @@
   <!--//////////-->
   <BehaviorTree
     ID="Constrained Pick and Place Subtree"
-    _description="Takes in waypoints to run a constrained pick and place operation."
+    _description="Takes in pick and place waypoints, auto-computes pre-positions using forward kinematics with a height offset, and uses force-controlled lowering for placement."
     _favorite="false"
   >
-    <Control ID="Sequence">
-      <!--Move to pick location-->
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="{pre_pick}"
-        joint_group_name="manipulator"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="{pick}"
-        joint_group_name="manipulator"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        keep_orientation_link_names="grasp_link"
-      />
-      <!--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-->
-      <SubTree
-        ID="Move to Waypoint"
-        joint_group_name="manipulator"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        _collapsed="true"
-        waypoint_name="{pre_pick}"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree
-        ID="Move to Waypoint"
-        joint_group_name="manipulator"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        _collapsed="true"
-        waypoint_name="{pre_place}"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree
-        ID="Move to Waypoint"
-        joint_group_name="manipulator"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        _collapsed="true"
-        waypoint_name="{place}"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree ID="Open Gripper" />
-      <SubTree
-        ID="Move to Waypoint"
-        joint_group_name="manipulator"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        _collapsed="true"
-        waypoint_name="{pre_place}"
-        keep_orientation_link_names="grasp_link"
-      />
+    <Control ID="Sequence" name="Main Sequence">
+      <!--Compute pre-pick and pre-place poses from waypoint FK + height offset (0.305m = 12 inches)-->
+      <Control ID="Sequence" name="Compute Pre-Positions">
+        <Action
+          ID="GetCurrentPlanningScene"
+          planning_scene_msg="{planning_scene}"
+        />
+        <!--Compute pre-pick pose: FK from pick waypoint, then offset upward-->
+        <Action
+          ID="RetrieveWaypoint"
+          joint_group_name="manipulator"
+          waypoint_joint_state="{pick_joint_state}"
+          waypoint_name="{pick}"
+        />
+        <Action
+          ID="UnpackRobotJointStateMessage"
+          joint_state="{pick_joint_state_msg}"
+          multi_dof_joint_state="{pick_multi_dof}"
+          robot_joint_state="{pick_joint_state}"
+        />
+        <Action
+          ID="ComputeLinkPoseForwardKinematics"
+          joint_state="{pick_joint_state_msg}"
+          link_name="grasp_link"
+          link_pose="{pick_link_pose}"
+          planning_scene="{planning_scene}"
+        />
+        <Action
+          ID="TransformPose"
+          input_pose="{pick_link_pose}"
+          output_pose="{pre_pick_pose}"
+          quaternion_xyzw="0;0;0;1"
+          translation_xyz="0;0;-0.305"
+          visualize_pose="true"
+          marker_text="Pre-Pick"
+        />
+        <!--Compute pre-place pose: FK from place waypoint, then offset upward-->
+        <Action
+          ID="RetrieveWaypoint"
+          joint_group_name="manipulator"
+          waypoint_joint_state="{place_joint_state}"
+          waypoint_name="{place}"
+        />
+        <Action
+          ID="UnpackRobotJointStateMessage"
+          joint_state="{place_joint_state_msg}"
+          multi_dof_joint_state="{place_multi_dof}"
+          robot_joint_state="{place_joint_state}"
+        />
+        <Action
+          ID="ComputeLinkPoseForwardKinematics"
+          joint_state="{place_joint_state_msg}"
+          link_name="grasp_link"
+          link_pose="{place_link_pose}"
+          planning_scene="{planning_scene}"
+        />
+        <Action
+          ID="TransformPose"
+          input_pose="{place_link_pose}"
+          output_pose="{pre_place_pose}"
+          quaternion_xyzw="0;0;0;1"
+          translation_xyz="0;0;-0.305"
+          marker_text="Pre-Place"
+          visualize_pose="true"
+        />
+      </Control>
+      <!--Move to pre-pick, then lower to pick using constrained motion-->
+      <Control ID="Sequence">
+        <Control ID="Sequence" name="Pick">
+          <Action
+            ID="InitializeMTCTask"
+            task_id="move_to_pre_pick"
+            controller_names="joint_trajectory_admittance_controller;robotiq_gripper_controller"
+            task="{pre_pick_task}"
+          />
+          <Action ID="SetupMTCCurrentState" task="{pre_pick_task}" />
+          <Action
+            ID="SetupMTCPlanToPose"
+            ik_frame="grasp_link"
+            planning_group_name="manipulator"
+            target_pose="{pre_pick_pose}"
+            task="{pre_pick_task}"
+          />
+          <Action
+            ID="PlanMTCTask"
+            solution="{pre_pick_solution}"
+            task="{pre_pick_task}"
+            max_solutions="1"
+          />
+          <SubTree ID="Execute MTC Solution" solution="{pre_pick_solution}" />
+          <SubTree
+            ID="Move to Waypoint"
+            waypoint_name="{pick}"
+            joint_group_name="manipulator"
+            keep_orientation="true"
+            keep_orientation_tolerance="0.2"
+            keep_orientation_link_names="grasp_link"
+          />
+          <!--ForceSuccess because the gripper closes to a position it will never reach (fingers fully closed)-->
+          <Decorator ID="ForceSuccess">
+            <SubTree ID="Close Gripper" />
+          </Decorator>
+        </Control>
+      </Control>
+      <!--Retract upward after grasping (Cartesian path, 0.305m = 12 inches)-->
+      <Control ID="Sequence" name="Pick Retract">
+        <Action
+          ID="CreateStampedPose"
+          reference_frame="grasp_link"
+          stamped_pose="{pick_retract_pose}"
+          position_xyz="0;0;-0.305"
+        />
+        <Action ID="ResetPoseStampedVector" vector="{pick_retract_path}" />
+        <Action
+          ID="AddPoseStampedToVector"
+          input="{pick_retract_pose}"
+          vector="{pick_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="{pick_retract_path}"
+          planning_group_name="manipulator"
+          position_only="true"
+          tip_links="grasp_link"
+          trajectory_sampling_rate="100"
+          velocity_scale_factor="1.000000"
+        />
+        <Action ID="ExecuteTrajectory" />
+      </Control>
+      <!--Move to pre-place position-->
+      <Control ID="Sequence">
+        <Control ID="Sequence" name="Move to Pre-Place">
+          <Action
+            ID="InitializeMTCTask"
+            task_id="move_to_pre_place"
+            controller_names="joint_trajectory_admittance_controller;robotiq_gripper_controller"
+            task="{pre_place_task}"
+          />
+          <Action ID="SetupMTCCurrentState" task="{pre_place_task}" />
+          <Action
+            ID="SetupMTCPlanToPose"
+            ik_frame="grasp_link"
+            planning_group_name="manipulator"
+            target_pose="{pre_place_pose}"
+            task="{pre_place_task}"
+          />
+          <Action
+            ID="PlanMTCTask"
+            solution="{pre_place_solution}"
+            task="{pre_place_task}"
+            max_solutions="1"
+          />
+          <SubTree ID="Execute MTC Solution" solution="{pre_place_solution}" />
+        </Control>
+      </Control>
+      <!--Force-controlled lower until contact (force threshold: 20N)-->
+      <Control ID="Sequence" name="Force-Controlled Place">
+        <Action
+          ID="SwitchController"
+          activate_controllers="velocity_force_controller"
+        />
+        <Action
+          ID="CallTriggerService"
+          service_name="/velocity_force_controller/zero_fts"
+        />
+        <Action
+          ID="CreateStampedWrench"
+          stamped_wrench="{desired_wrench}"
+          reference_frame="grasp_link"
+          torque_xyz="0;0;0"
+          force_xyz="0;0;0"
+        />
+        <Action
+          ID="CreateStampedTwist"
+          reference_frame="grasp_link"
+          stamped_twist="{desired_twist}"
+          angular_velocity_xyz="0;0;0"
+          linear_velocity_xyz="0.0;0;0.05"
+        />
+        <Action
+          ID="CreateStampedPose"
+          reference_frame="grasp_link"
+          stamped_pose="{initial_pose}"
+        />
+        <Action
+          ID="TransformPoseFrame"
+          output_pose="{initial_pose}"
+          input_pose="{initial_pose}"
+        />
+        <Control ID="Parallel" failure_count="1" success_count="1">
+          <Action
+            ID="ForceExceedsThreshold"
+            minimum_consecutive_wrench_values="5"
+            wrench_frame_name="tool0"
+            hand_frame_name="grasp_link"
+            force_threshold="100"
+          />
+          <Action
+            ID="PublishVelocityForceCommand"
+            force_controlled_axes="0;0;0;0;0;0"
+            wrench_gain="0.01"
+            velocity_controlled_axes="0;0;1;0;0;0"
+            publish_rate="50"
+            twist_stamped="{desired_twist}"
+            wrench_stamped="{desired_wrench}"
+          />
+        </Control>
+        <SubTree
+          ID="Force Relaxation"
+          _collapsed="true"
+          wrench_gain="0.01"
+          relaxation_time_ms="500"
+          tip_link="grasp_link"
+        />
+        <Action
+          ID="SwitchController"
+          activate_controllers="joint_trajectory_admittance_controller"
+        />
+      </Control>
+      <!--Release and retract upward-->
+      <Control ID="Sequence">
+        <Control ID="Sequence" name="Release and Retract">
+          <SubTree ID="Open Gripper" />
+          <Action
+            ID="CreateStampedPose"
+            reference_frame="grasp_link"
+            stamped_pose="{place_retract_pose}"
+            position_xyz="0;0;-0.08"
+          />
+          <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"
+          />
+          <Action ID="ExecuteTrajectory" />
+        </Control>
+      </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Constrained Pick and Place Subtree">
-      <inout_port name="pick" default="" />
-      <inout_port name="place" default="" />
-      <inout_port name="pre_pick" default="" />
-      <inout_port name="pre_place" default="" />
       <MetadataFields>
         <Metadata runnable="false" />
         <Metadata subcategory="Motion - Execute" />
       </MetadataFields>
+      <input_port name="pick" default="" />
+      <input_port name="place" default="" />
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/constrained_pick_place.xml b/src/lab_sim/objectives/constrained_pick_place.xml
index 0b435156..6d9a2346 100644
--- a/src/lab_sim/objectives/constrained_pick_place.xml
+++ b/src/lab_sim/objectives/constrained_pick_place.xml
@@ -3,42 +3,22 @@
   <!--//////////-->
   <BehaviorTree
     ID="Move Flasks to Burners"
-    _description="Example of picking and placing objects with orientation constraints enforced on the arm's motion"
+    _description="Example of picking and placing objects with orientation constraints and force-controlled placement"
     _favorite="true"
   >
     <Control ID="Sequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        waypoint_name="Flask 1a - Pre Pick"
-        joint_group_name="manipulator"
-        keep_orientation_link_names="grasp_link"
-      />
       <SubTree ID="Open Gripper" />
       <SubTree
         ID="Constrained Pick and Place Subtree"
-        pre_pick="Flask 1a - Pre Pick"
-        pick="Flask 1b - Pick"
-        pre_place="Flask 1c - Pre Place"
-        place="Flask 1d - Place"
+        pick="Flask 1 - Pick"
+        place="Flask 1 - Place"
         name="Flask 1"
       />
       <SubTree
         ID="Constrained Pick and Place Subtree"
-        pre_pick="Flask 3a - Pre Pick"
-        pick="Flask 3b - Pick"
-        pre_place="Flask 3c - Pre Place"
-        place="Flask 3d - Place"
-        name="Flask 3"
-      />
-      <SubTree
-        ID="Constrained Pick and Place Subtree"
-        pre_pick="Flask 2a - Pre Pick"
-        pick="Flask 2b - Pick"
-        pre_place="Flask 2c - Pre Place"
-        place="Flask 2d - Place"
+        pick="Flask 2 - Pick"
+        place="Flask 2 - Place"
         name="Flask 2"
-        _collapsed="true"
       />
     </Control>
   </BehaviorTree>
diff --git a/src/lab_sim/objectives/cycle_between_waypoints.xml b/src/lab_sim/objectives/cycle_between_waypoints.xml
index 477b9bcb..aea34e58 100644
--- a/src/lab_sim/objectives/cycle_between_waypoints.xml
+++ b/src/lab_sim/objectives/cycle_between_waypoints.xml
@@ -9,7 +9,7 @@
       <Control ID="Sequence">
         <SubTree
           ID="Move to Waypoint"
-          waypoint_name="Above Pick Cube"
+          waypoint_name="Above Pick Object"
           joint_group_name="manipulator"
         />
         <SubTree
diff --git a/src/lab_sim/objectives/get_candidate_grasps_subtree.xml b/src/lab_sim/objectives/get_candidate_grasps_subtree.xml
index 31087551..715c43dc 100644
--- a/src/lab_sim/objectives/get_candidate_grasps_subtree.xml
+++ b/src/lab_sim/objectives/get_candidate_grasps_subtree.xml
@@ -4,7 +4,6 @@
   <BehaviorTree
     ID="Get Candidate Grasps Subtree"
     _description="Infer grasp poses from camera image."
-    _favorite="false"
   >
     <Control ID="Sequence">
       <Action ID="ClearSnapshot" />
diff --git a/src/lab_sim/objectives/grasp_planning.xml b/src/lab_sim/objectives/grasp_planning.xml
index 34038f2e..597d3ca4 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/load_mesh_as_green_poincloud.xml b/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml
deleted file mode 100644
index ae09e2ea..00000000
--- a/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml
+++ /dev/null
@@ -1,40 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Load Mesh as Green Point Cloud">
-  <!--//////////-->
-  <BehaviorTree
-    ID="Load Mesh as Green Point Cloud"
-    _description="Loads a point cloud from a file and colors it green."
-    _favorite="false"
-  >
-    <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="LoadPointCloudFromFile"
-        color="0;255;0"
-        package_name="lab_sim"
-        file_path="description/assets/Cube.stl"
-        point_cloud="{point_cloud}"
-      />
-      <Action
-        ID="TransformPointCloudFrame"
-        input_cloud="{point_cloud}"
-        output_cloud="{point_cloud}"
-      />
-      <Action
-        ID="TransformPointCloud"
-        input_cloud="{point_cloud}"
-        transform_pose="{initial_pose}"
-      />
-    </Control>
-  </BehaviorTree>
-  <TreeNodesModel>
-    <SubTree ID="Load Mesh as Green Point Cloud">
-      <output_port name="point_cloud" default="{green_cloud}" />
-      <input_port name="_collapsed" default="true" />
-      <input_port name="initial_pose" default="{stamped_pose}" />
-      <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
-        <Metadata runnable="false" />
-      </MetadataFields>
-    </SubTree>
-  </TreeNodesModel>
-</root>
diff --git a/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml b/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml
deleted file mode 100644
index 29dfd62d..00000000
--- a/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml
+++ /dev/null
@@ -1,39 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Load Mesh as Red Point Cloud">
-  <!--//////////-->
-  <BehaviorTree
-    ID="Load Mesh as Red Point Cloud"
-    _description="Loads a mesh from a file and colors it red."
-    _favorite="false"
-  >
-    <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="LoadPointCloudFromFile"
-        color="255;0;0"
-        package_name="lab_sim"
-        file_path="description/assets/Cube.stl"
-        point_cloud="{point_cloud}"
-      />
-      <Action
-        ID="TransformPointCloudFrame"
-        input_cloud="{point_cloud}"
-        output_cloud="{point_cloud}"
-      />
-      <Action
-        ID="TransformPointCloud"
-        input_cloud="{point_cloud}"
-        transform_pose="{initial_pose}"
-      />
-    </Control>
-  </BehaviorTree>
-  <TreeNodesModel>
-    <SubTree ID="Load Mesh as Red Point Cloud">
-      <output_port name="point_cloud" default="{red_cloud}" />
-      <input_port name="initial_pose" default="{stamped_pose}" />
-      <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
-        <Metadata runnable="false" />
-      </MetadataFields>
-    </SubTree>
-  </TreeNodesModel>
-</root>
diff --git a/src/lab_sim/objectives/move_along_square.xml b/src/lab_sim/objectives/move_along_square.xml
index 6c8b72fb..c5292a81 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 fa7571e2..8ee08b70 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 b468d3a7..eedf4f29 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 a0509e5d..7fda1631 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_1_pill_bottle.xml b/src/lab_sim/objectives/pick_1_pill_bottle_with_ml.xml
similarity index 91%
rename from src/lab_sim/objectives/pick_1_pill_bottle.xml
rename to src/lab_sim/objectives/pick_1_pill_bottle_with_ml.xml
index 19c048cf..b3d9711c 100644
--- a/src/lab_sim/objectives/pick_1_pill_bottle.xml
+++ b/src/lab_sim/objectives/pick_1_pill_bottle_with_ml.xml
@@ -1,8 +1,8 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Pick 1 Pill Bottle">
+<root BTCPP_format="4" main_tree_to_execute="Pick 1 Pill Bottle with ML">
   <!--//////////-->
   <BehaviorTree
-    ID="Pick 1 Pill Bottle"
+    ID="Pick 1 Pill Bottle with ML"
     _description="Captures and segments objects from a point cloud based on a text prompt. Then grasps the first object returned from the segmentation."
     _favorite="true"
   >
@@ -13,7 +13,7 @@
         _collapsed="true"
         joint_group_name="manipulator"
         keep_orientation_link_names="grasp_link"
-        waypoint_name="Above Pick Cube"
+        waypoint_name="Look at Table"
       />
       <SubTree
         ID="Segment Point Cloud from Text Prompt Subtree"
@@ -28,7 +28,7 @@
         clip_model_path="models/clip.onnx"
         clipseg_model_path="models/clipseg.onnx"
         erosion_size="2"
-        prompts="a cylindrical object with a dark gray and silver metallic body, featuring a circular blue cap on top"
+        prompts="a cylindrical object"
         threshold="0.28"
         masks3d="{masks3d}"
         point_cloud="{point_cloud}"
@@ -105,12 +105,12 @@
         _collapsed="true"
         joint_group_name="manipulator"
         keep_orientation_link_names="grasp_link"
-        waypoint_name="Above Pick Cube"
+        waypoint_name="Look at Table"
       />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Pick 1 Pill Bottle">
+    <SubTree ID="Pick 1 Pill Bottle with ML">
       <MetadataFields>
         <Metadata runnable="true" />
         <Metadata subcategory="Application - ML (GPU Recommended)" />
diff --git a/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml b/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml
new file mode 100644
index 00000000..4805ed2e
--- /dev/null
+++ b/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml
@@ -0,0 +1,578 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Pick All Bottles with AprilTags">
+  <BehaviorTree
+    ID="Pick All Bottles with AprilTags"
+    _description="Detects AprilTags on medicine bottles, picks them up, and places them in a tray. Loops until no more tags are found."
+    _favorite="true"
+  >
+    <Control ID="Sequence">
+      <!--Temporary: reset simulation for debugging-->
+      <Action
+        ID="ResetMujocoKeyframe"
+        keyframe_name="default"
+        response_timeout="10.0"
+        wait_for_server_available_timeout="3.0"
+      />
+      <!--Add collision wall at the back of the table to prevent the arm from hitting the shelf-->
+      <Action
+        ID="GetCurrentPlanningScene"
+        planning_scene_msg="{planning_scene}"
+      />
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="world"
+        stamped_pose="{back_wall_pose}"
+        position_xyz="0.15; 0.78; 0.72"
+      />
+      <Action
+        ID="CreateGraspableObject"
+        cuboid_object="{back_wall_object}"
+        dx="2.0"
+        dy="0.05"
+        dz="0.75"
+        generate_front_face="false"
+        generate_side_faces="false"
+        generate_top_face="false"
+        object_id="back_wall"
+        pose="{back_wall_pose}"
+      />
+      <Action
+        ID="AddVirtualObjectToPlanningScene"
+        apply_planning_scene_service="/apply_planning_scene"
+        object="{back_wall_object}"
+        planning_scene_msg="{planning_scene}"
+      />
+      <Action
+        ID="SwitchUIPrimaryView"
+        primary_view_name="/apriltag_detections"
+      />
+      <Control
+        ID="Sequence"
+        name="Compute Tray Place Positions"
+        _collapsed="true"
+      >
+        <SubTree ID="Open Gripper" _collapsed="true" />
+        <SubTree
+          ID="Move to Waypoint"
+          waypoint_name="Predrop Bottles Right"
+          _collapsed="true"
+        />
+        <Action
+          ID="GetCameraInfo"
+          topic_name="/wrist_camera/camera_info"
+          message_out="{camera_info}"
+          message_timeout_sec="5.0"
+          publisher_timeout_sec="5.0"
+        />
+        <Action
+          ID="GetImage"
+          topic_name="/wrist_camera/color"
+          message_out="{tray_image}"
+          message_timeout_sec="5.0"
+        />
+        <Action
+          ID="DetectAprilTags"
+          image="{tray_image}"
+          camera_info="{camera_info}"
+          detections="{tray_detections}"
+          apriltag_family_name="36h11"
+          max_hamming="0"
+          n_threads="1"
+          quad_decimate="2.0"
+          quad_sigma="0.0"
+          refine_edges="true"
+          tag_size="0.08"
+        />
+        <Action
+          ID="ComputeTrayPlacePositionsUsingAprilTags"
+          detections="{tray_detections}"
+          camera_info="{camera_info}"
+          input_image="{tray_image}"
+          tray_apriltag_id="1"
+          num_rows="3"
+          columns_per_row="2"
+          row_spacing="0.10"
+          column_spacing="0.09"
+          tag_to_tray_center="0.15"
+          bottle_diameter="0.04"
+          place_positions="{place_positions}"
+        />
+        <!--Visualize coordinate frames at every place location-->
+        <Action ID="Script" code="place_marker_count := 0" />
+        <Decorator
+          ID="ForEach"
+          vector_in="{place_positions}"
+          out="{place_viz_pose}"
+          index="{place_viz_index}"
+        >
+          <Control ID="Sequence">
+            <Action
+              ID="VisualizePose"
+              pose="{place_viz_pose}"
+              marker_size="0.05"
+              marker_lifetime="0"
+              marker_name="{place_marker_count}"
+              marker_text="Place"
+            />
+            <Action ID="Script" code="place_marker_count += 1" />
+          </Control>
+        </Decorator>
+      </Control>
+      <Control ID="Sequence">
+        <SubTree ID="Clear Snapshot" _collapsed="true" />
+        <SubTree ID="Take Wrist Camera Snapshot" _collapsed="true" />
+        <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">
+        <Action
+          ID="SwitchController"
+          activate_controllers="velocity_force_controller"
+        />
+        <Action
+          ID="CallTriggerService"
+          service_name="/velocity_force_controller/zero_fts"
+        />
+        <Action
+          ID="SwitchController"
+          activate_controllers="joint_trajectory_admittance_controller"
+        />
+      </Control>
+      <Decorator ID="ForceSuccess">
+        <Decorator ID="KeepRunningUntilFailure">
+          <Control ID="Sequence" name="TopLevelSequence" _collapsed="true">
+            <Control ID="Sequence" name="Detect AprilTag" _collapsed="true">
+              <SubTree ID="Look at Table" _collapsed="true" />
+              <SubTree ID="Open Gripper" _collapsed="false" />
+              <Action
+                ID="GetCameraInfo"
+                topic_name="/wrist_camera/camera_info"
+                message_out="{camera_info}"
+                message_timeout_sec="5.000000"
+                publisher_timeout_sec="5.000000"
+              />
+              <Action
+                ID="GetImage"
+                topic_name="/wrist_camera/color"
+                message_out="{image}"
+                message_timeout_sec="5.000000"
+              />
+              <!--Detect AprilTag and compute a pre-grasp viewpoint offset above it-->
+              <Action
+                ID="DetectAprilTags"
+                image="{image}"
+                camera_info="{camera_info}"
+                detections="{detections}"
+                apriltag_family_name="36h11"
+                max_hamming="0"
+                n_threads="1"
+                quad_decimate="2.0"
+                quad_sigma="0.0"
+                refine_edges="true"
+                tag_size="0.036"
+              />
+              <Action
+                ID="GetCenterMostAprilTag"
+                detection_pose="{detection_pose}"
+                detections="{detections}"
+                camera_info="{camera_info}"
+                image="{image}"
+                tag_size="0.036"
+              />
+              <Action
+                ID="TransformPoseFrame"
+                input_pose="{detection_pose}"
+                output_pose="{detection_pose}"
+                target_frame_id="world"
+              />
+              <Action
+                ID="TransformPose"
+                output_pose="{view_pose}"
+                input_pose="{detection_pose}"
+                quaternion_xyzw="1;0;0;0"
+                translation_xyz="0.0;0.0;0.3"
+                visualize_pose="true"
+                marker_size="0.1"
+                marker_lifetime="0"
+                marker_text="Pre-Grasp"
+              />
+            </Control>
+            <Control ID="Sequence" name="Refine Pose" _collapsed="true">
+              <Control ID="Sequence">
+                <!--Move to the pre-grasp viewpoint above the detected tag-->
+                <SubTree
+                  ID="Plan Move To Pose"
+                  target_pose="{view_pose}"
+                  move_to_pose_solution="{move_to_pose_solution}"
+                  _collapsed="true"
+                />
+                <SubTree
+                  ID="Execute MTC Solution"
+                  solution="{move_to_pose_solution}"
+                  _collapsed="true"
+                />
+                <!--Re-detect the tag from closer range for a more accurate pose-->
+                <Action
+                  ID="GetImage"
+                  topic_name="/wrist_camera/color"
+                  message_out="{image}"
+                  message_timeout_sec="5.000000"
+                />
+                <Action
+                  ID="DetectAprilTags"
+                  image="{image}"
+                  camera_info="{camera_info}"
+                  detections="{detections}"
+                  apriltag_family_name="36h11"
+                  max_hamming="0"
+                  n_threads="1"
+                  quad_decimate="2.0"
+                  quad_sigma="0.0"
+                  refine_edges="true"
+                  tag_size="0.036"
+                />
+                <Action
+                  ID="GetCenterMostAprilTag"
+                  detection_pose="{detection_pose}"
+                  detections="{detections}"
+                  camera_info="{camera_info}"
+                  image="{image}"
+                  tag_size="0.036"
+                />
+                <Action
+                  ID="TransformPoseFrame"
+                  input_pose="{detection_pose}"
+                  output_pose="{detection_pose}"
+                  target_frame_id="world"
+                />
+                <Action
+                  ID="VisualizePose"
+                  pose="{detection_pose}"
+                  marker_size="0.05"
+                  marker_lifetime="0"
+                  marker_name="detection_raw"
+                  marker_text="Detection Raw"
+                />
+                <!--Override tag orientation with a fixed downward grasp and offset Z to the bottle body-->
+                <Action
+                  ID="OverridePoseOrientation"
+                  input_pose="{detection_pose}"
+                  output_pose="{offset_pose}"
+                  orientation_xyzw="1;0;0;0"
+                />
+                <Action
+                  ID="TransformPose"
+                  input_pose="{offset_pose}"
+                  output_pose="{offset_pose}"
+                  translation_xyz="0;0;-0.03"
+                />
+                <!--Calibration offset: tune translation_xyz to correct systematic XY error after table move. Local X = World +X, Local Y = World -Y (orientation is 180° around X).-->
+                <Action
+                  ID="TransformPose"
+                  input_pose="{offset_pose}"
+                  output_pose="{offset_pose}"
+                  quaternion_xyzw="0.000; 0.000; 1.000; 0.000"
+                  translation_xyz="0;0;0"
+                />
+                <Action
+                  ID="VisualizeLine"
+                  start_pose="{detection_pose}"
+                  end_pose="{offset_pose}"
+                  line_thickness="0.005"
+                  marker_lifetime="0"
+                />
+                <Action
+                  ID="VisualizeLine"
+                  start_pose="{view_pose}"
+                  end_pose="{offset_pose}"
+                  line_thickness="0.005"
+                  marker_lifetime="0"
+                />
+              </Control>
+            </Control>
+            <Control ID="Sequence" name="Pick from Pose" _collapsed="true">
+              <Control ID="Sequence">
+                <Control ID="Sequence">
+                  <!--Initialize MTC pick task-->
+                  <Action
+                    ID="InitializeMTCTask"
+                    controller_names="joint_trajectory_admittance_controller;robotiq_gripper_controller"
+                    task="{mtc_task}"
+                    task_id="pick_object"
+                    trajectory_monitoring="false"
+                    timeout="-1.000000"
+                  />
+                  <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
+                  <Action
+                    ID="SetupMTCConnectWithProRRT"
+                    acceleration_scale_factor="1.000000"
+                    keep_orientation="false"
+                    keep_orientation_link_names="grasp_link"
+                    keep_orientation_tolerance="0.100000"
+                    link_padding="0.000000"
+                    max_iterations="20000"
+                    planning_group_name="manipulator"
+                    seed="0"
+                    task="{mtc_task}"
+                    trajectory_sampling_rate="100"
+                    velocity_scale_factor="1.000000"
+                  />
+                  <!--Approach along grasp_link Z axis-->
+                  <Action
+                    ID="SetupMTCMoveAlongFrameAxis"
+                    acceleration_scale="1.000000"
+                    axis_frame="grasp_link"
+                    axis_x="0.000000"
+                    axis_y="0.000000"
+                    axis_z="1.000000"
+                    hand_frame="grasp_link"
+                    max_distance="0.1"
+                    min_distance="0.1"
+                    planning_group_name="manipulator"
+                    task="{mtc_task}"
+                    velocity_scale="1.000000"
+                    ignore_environment_collisions="false"
+                  />
+                  <!--Solve IK for the grasp pose-->
+                  <Action
+                    ID="ResetPoseStampedVector"
+                    vector="{grasp_pose_vector}"
+                  />
+                  <Action
+                    ID="AddPoseStampedToVector"
+                    input="{offset_pose}"
+                    vector="{grasp_pose_vector}"
+                  />
+                  <Action
+                    ID="SetupMTCBatchPoseIK"
+                    end_effector_group="moveit_ee"
+                    end_effector_link="grasp_link"
+                    ik_group="manipulator"
+                    ik_timeout_s="0.1"
+                    max_ik_solutions="16"
+                    monitored_stage="current state"
+                    target_poses="{grasp_pose_vector}"
+                    task="{mtc_task}"
+                  />
+                  <Action
+                    ID="VisualizePose"
+                    pose="{offset_pose}"
+                    marker_size="0.05"
+                    marker_lifetime="0"
+                    marker_name="grasp_target"
+                    marker_text="Grasp Target"
+                  />
+                  <Action ID="BreakpointSubscriber" _skipIf="true" />
+                  <Action
+                    ID="PlanMTCTask"
+                    solution="{mtc_solution}"
+                    task="{mtc_task}"
+                    max_solutions="4"
+                  />
+                  <SubTree
+                    ID="Execute MTC Solution"
+                    solution="{mtc_solution}"
+                    _collapsed="true"
+                  />
+                  <Decorator ID="ForceSuccess">
+                    <Action
+                      ID="MoveGripperAction"
+                      gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
+                      position="0.58"
+                      timeout="15.0"
+                    />
+                  </Decorator>
+                </Control>
+              </Control>
+            </Control>
+            <Control ID="Sequence" name="Retract" _collapsed="true">
+              <!--Retract away from the grasp-->
+              <Action
+                ID="CreateStampedPose"
+                reference_frame="grasp_link"
+                stamped_pose="{retract_pose}"
+                position_xyz="0;0;-0.25"
+              />
+              <Action ID="ResetPoseStampedVector" vector="{retract_path}" />
+              <Action
+                ID="AddPoseStampedToVector"
+                input="{retract_pose}"
+                vector="{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="{retract_path}"
+                planning_group_name="manipulator"
+                position_only="true"
+                tip_links="grasp_link"
+                trajectory_sampling_rate="100"
+                velocity_scale_factor="1.000000"
+              />
+              <Action ID="ExecuteTrajectory" />
+            </Control>
+            <Control ID="Sequence" name="Place in Tray" _collapsed="true">
+              <!--Pop the next place pose from the front of the computed positions vector-->
+              <Action
+                ID="GetElementOfVector"
+                vector_in="{place_positions}"
+                index="0"
+                element="{current_place_pose}"
+              />
+              <!--Pause here to inspect the place pose before lowering-->
+              <!--Compute a pre-place hover pose above the target-->
+              <Action
+                ID="TransformPose"
+                input_pose="{current_place_pose}"
+                output_pose="{pre_place_pose}"
+                quaternion_xyzw="0;0;1;0"
+                translation_xyz="0;0;-0.4"
+                visualize_pose="true"
+                marker_text="Pre-Place Pose"
+              />
+              <!--Move to the pre-place hover position above the tray-->
+              <SubTree
+                ID="Plan Move To Pose"
+                target_pose="{pre_place_pose}"
+                move_to_pose_solution="{place_solution}"
+                _collapsed="true"
+              />
+              <SubTree
+                ID="Execute MTC Solution"
+                solution="{place_solution}"
+                _collapsed="true"
+              />
+              <!--Lower until contact: flattened from lab_sim Push Button (velocity/force controller)-->
+              <Control ID="Sequence">
+                <Action
+                  ID="SwitchController"
+                  activate_controllers="velocity_force_controller"
+                />
+                <Action
+                  ID="CallTriggerService"
+                  service_name="/velocity_force_controller/zero_fts"
+                />
+                <Action
+                  ID="CreateStampedWrench"
+                  stamped_wrench="{desired_wrench}"
+                  reference_frame="grasp_link"
+                  torque_xyz="0;0;0"
+                  force_xyz="0;0;0"
+                />
+                <Action
+                  ID="CreateStampedTwist"
+                  reference_frame="grasp_link"
+                  stamped_twist="{desired_twist}"
+                  angular_velocity_xyz="0;0;0"
+                  linear_velocity_xyz="0.0;0;0.05"
+                />
+                <Action
+                  ID="CreateStampedPose"
+                  reference_frame="grasp_link"
+                  stamped_pose="{initial_pose}"
+                />
+                <Action
+                  ID="TransformPoseFrame"
+                  output_pose="{initial_pose}"
+                  input_pose="{initial_pose}"
+                />
+                <Control ID="Parallel" failure_count="1" success_count="1">
+                  <Action
+                    ID="ForceExceedsThreshold"
+                    minimum_consecutive_wrench_values="5"
+                    wrench_frame_name="tool0"
+                    hand_frame_name="grasp_link"
+                    force_threshold="30"
+                  />
+                  <Action
+                    ID="PublishVelocityForceCommand"
+                    force_controlled_axes="0;0;0;0;0;0"
+                    wrench_gain="0.01"
+                    velocity_controlled_axes="0;0;1;0;0;0"
+                    publish_rate="50"
+                    twist_stamped="{desired_twist}"
+                    wrench_stamped="{desired_wrench}"
+                  />
+                </Control>
+                <SubTree
+                  ID="Force Relaxation"
+                  _collapsed="true"
+                  wrench_gain="0.01"
+                  relaxation_time_ms="500"
+                  tip_link="grasp_link"
+                />
+                <Action
+                  ID="SwitchController"
+                  activate_controllers="joint_trajectory_admittance_controller"
+                />
+              </Control>
+            </Control>
+            <Control ID="Sequence" name="Release and Retract" _collapsed="true">
+              <Control ID="Sequence">
+                <Control ID="Sequence">
+                  <Control ID="Sequence">
+                    <!--Release the bottle-->
+                    <SubTree ID="Open Gripper" _collapsed="true" />
+                    <!--Retract upward after releasing the bottle-->
+                    <Action
+                      ID="CreateStampedPose"
+                      reference_frame="grasp_link"
+                      stamped_pose="{place_retract_pose}"
+                      position_xyz="0;0;-0.08"
+                    />
+                    <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"
+                    />
+                    <Action ID="ExecuteTrajectory" />
+                    <!--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>
+            </Control>
+          </Control>
+        </Decorator>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Pick All Bottles with AprilTags">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="AprilTag" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
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 6d2ad6b2..00000000
--- 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_april_tag_object_with_approval.xml b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
index 83d262b3..38cbe30e 100644
--- a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
+++ b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
@@ -41,7 +41,9 @@
         output_pose="{view_pose}"
         input_pose="{detection_pose}"
         quaternion_xyzw="1;0;0;0"
-        translation_xyz="0;0.1;0.15"
+        translation_xyz="0;0.1;0.3"
+        visualize_pose="true"
+        marker_text="Pre-Pick"
       />
       <SubTree
         ID="Plan Move To Pose"
diff --git a/src/lab_sim/objectives/pick_from_pose.xml b/src/lab_sim/objectives/pick_from_pose.xml
index e73286bb..320baa47 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 aedfe8da..dbfc6599 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 fad39d50..3186a75e 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 0862561b..c50a9566 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 3ac6b6dc..981ac792 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/plan_move_to_pose.xml b/src/lab_sim/objectives/plan_move_to_pose.xml
index b5cea06c..8eab4a91 100644
--- a/src/lab_sim/objectives/plan_move_to_pose.xml
+++ b/src/lab_sim/objectives/plan_move_to_pose.xml
@@ -25,20 +25,21 @@
         ID="PlanMTCTask"
         solution="{move_to_pose_solution}"
         task="{move_to_pose_task}"
+        max_solutions="4"
       />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Plan Move To Pose">
-      <input_port name="target_pose" default="{target_pose}" />
-      <output_port
-        name="move_to_pose_solution"
-        default="{move_to_pose_solution}"
-      />
       <MetadataFields>
         <Metadata subcategory="Motion - Planning" />
         <Metadata runnable="false" />
       </MetadataFields>
+      <output_port
+        name="move_to_pose_solution"
+        default="{move_to_pose_solution}"
+      />
+      <input_port name="target_pose" default="{target_pose}" />
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/register_cad_part.xml b/src/lab_sim/objectives/register_cad_part.xml
index 54487b00..eb2a79f9 100644
--- a/src/lab_sim/objectives/register_cad_part.xml
+++ b/src/lab_sim/objectives/register_cad_part.xml
@@ -11,19 +11,32 @@
       <SubTree ID="Look at Table" _collapsed="true" />
       <Action
         ID="CreateStampedPose"
+        reference_frame="world"
         position_xyz="0.02;.75;0.58"
         orientation_xyzw="0;0;0;1"
         stamped_pose="{stamped_pose}"
         name="Initial Guess"
       />
-      <SubTree
-        ID="Load Mesh as Red Point Cloud"
-        _collapsed="false"
-        output_cloud="{red_cloud}"
-        initial_pose="{stamped_pose}"
+      <Action
+        ID="LoadPointCloudFromFile"
+        color="255;0;0"
+        package_name="lab_sim"
+        file_path="description/assets/Cube.stl"
         point_cloud="{red_cloud}"
       />
+      <Action
+        ID="TransformPointCloudFrame"
+        input_cloud="{red_cloud}"
+        output_cloud="{red_cloud}"
+      />
+      <Action
+        ID="TransformPointCloud"
+        input_cloud="{red_cloud}"
+        transform_pose="{stamped_pose}"
+        output_cloud="{red_cloud}"
+      />
       <Action ID="SendPointCloudToUI" point_cloud="{red_cloud}" />
+      <Action ID="BreakpointSubscriber" />
       <Action
         ID="GetPointCloud"
         topic_name="/wrist_camera/points"
@@ -42,13 +55,24 @@
         max_correspondence_distance="0.5"
         max_iterations="100"
       />
-      <SubTree
-        ID="Load Mesh as Green Point Cloud"
-        _collapsed="false"
-        output_cloud="{green_cloud}"
-        initial_pose="{stamped_pose}"
+      <Action
+        ID="LoadPointCloudFromFile"
+        color="0;255;0"
+        package_name="lab_sim"
+        file_path="description/assets/Cube.stl"
         point_cloud="{green_cloud}"
       />
+      <Action
+        ID="TransformPointCloudFrame"
+        input_cloud="{green_cloud}"
+        output_cloud="{green_cloud}"
+      />
+      <Action
+        ID="TransformPointCloud"
+        input_cloud="{green_cloud}"
+        transform_pose="{stamped_pose}"
+        output_cloud="{green_cloud}"
+      />
       <Action
         ID="TransformPointCloud"
         input_cloud="{green_cloud}"
diff --git a/src/lab_sim/objectives/register_cad_part_subtree.xml b/src/lab_sim/objectives/register_cad_part_subtree.xml
index 2fee3666..fe5f3de7 100644
--- a/src/lab_sim/objectives/register_cad_part_subtree.xml
+++ b/src/lab_sim/objectives/register_cad_part_subtree.xml
@@ -17,13 +17,23 @@
         name="Initial Guess"
         reference_frame="world"
       />
-      <SubTree
-        ID="Load Mesh as Red Point Cloud"
-        _collapsed="false"
-        output_cloud="{red_cloud}"
-        initial_pose="{stamped_pose}"
+      <Action
+        ID="LoadPointCloudFromFile"
+        color="255;0;0"
+        package_name="lab_sim"
+        file_path="description/assets/Cube.stl"
         point_cloud="{red_cloud}"
       />
+      <Action
+        ID="TransformPointCloudFrame"
+        input_cloud="{red_cloud}"
+        output_cloud="{red_cloud}"
+      />
+      <Action
+        ID="TransformPointCloud"
+        input_cloud="{red_cloud}"
+        transform_pose="{stamped_pose}"
+      />
       <Action
         ID="SendPointCloudToUI"
         point_cloud="{red_cloud}"
@@ -55,13 +65,23 @@
         max_iterations="100"
         target_pose_in_base_frame="{target_pose}"
       />
-      <SubTree
-        ID="Load Mesh as Green Point Cloud"
-        _collapsed="false"
-        output_cloud="{green_cloud}"
-        initial_pose="{stamped_pose}"
+      <Action
+        ID="LoadPointCloudFromFile"
+        color="0;255;0"
+        package_name="lab_sim"
+        file_path="description/assets/Cube.stl"
         point_cloud="{green_cloud}"
       />
+      <Action
+        ID="TransformPointCloudFrame"
+        input_cloud="{green_cloud}"
+        output_cloud="{green_cloud}"
+      />
+      <Action
+        ID="TransformPointCloud"
+        input_cloud="{green_cloud}"
+        transform_pose="{stamped_pose}"
+      />
       <Action
         ID="TransformPointCloud"
         input_cloud="{green_cloud}"
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 16448fe6..00000000
--- 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 00000000..0f29542c
--- /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 0d7a55a5..0804e4c4 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 591effb6..26ddff1c 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" />
diff --git a/src/lab_sim/package.xml b/src/lab_sim/package.xml
index 86451190..6bddc807 100644
--- a/src/lab_sim/package.xml
+++ b/src/lab_sim/package.xml
@@ -28,6 +28,7 @@
   <exec_depend>robotiq_description</exec_depend>
   <exec_depend>ur_description</exec_depend>
   <exec_depend>velocity_force_controller</exec_depend>
+  <exec_depend>lab_sim_behaviors</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
 
diff --git a/src/lab_sim/waypoints/ur_waypoints.yaml b/src/lab_sim/waypoints/ur_waypoints.yaml
index a74e2aa0..c857a2a9 100644
--- a/src/lab_sim/waypoints/ur_waypoints.yaml
+++ b/src/lab_sim/waypoints/ur_waypoints.yaml
@@ -21,143 +21,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.06656709465883658
-    - 0.31742962464478663
-    - -0.07518824419393247
-    - -0.7207568718596512
-    - 0.8665668564352057
-    - -1.6837956156664982
-    - -1.5707507158288945
-    - -0.07517349313281775
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: Above Pick Cube
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.06656710337674145
-    - 0.1162925924535442
-    - 0.2054240147028693
-    - -0.987070953194143
-    - 1.3146849599773849
-    - -1.8698622612624394
-    - -1.5707050706852668
-    - 0.20529114323213696
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: Above Place Cube
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.7639671853765404
-    - 0.6551230244120644
-    - -2.597276923961381
-    - -1.7207992223871735
-    - -1.8054851963579275
-    - -2.9105970459798978
-    - -1.012936176528235
-    - 3.2513966966550787
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: Flask 1a - Pre Pick
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.7639662602883518
-    - 0.6416717485181368
-    - -2.4872028523038185
-    - -2.2797066973266604
-    - -2.135135409489318
-    - -2.0350474070457296
-    - -0.9043501601840921
-    - 3.2728332839692875
+    - 0.0026038249605273597
+    - -0.2150255024029047
+    - 0.11192462801491523
+    - -2.18036249863573
+    - 1.6587113787793821
+    - -1.4687347456884468
+    - -1.5919159056315824
+    - 0.08471562792466861
     velocity: []
   multi_dof_joint_state:
     header:
@@ -169,7 +40,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 1b - Pick
+  name: 3 Waypoint Mid Point
 - description: ''
   favorite: false
   joint_group_names:
@@ -193,14 +64,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639676351734388
-    - -0.14695746092055925
-    - -2.8650633002008017
-    - -1.8611906531500544
-    - -1.7478027758503683
-    - -2.810467735524929
-    - -1.2774332801079342
-    - 3.2091308552581377
+    - 0.052556448066187256
+    - 0.14423170107824168
+    - -0.07537379607283551
+    - -1.2866092520867383
+    - 2.438370923671909
+    - -2.7991165107946703
+    - -1.552715310086053
+    - -0.33811216558077
     velocity: []
   multi_dof_joint_state:
     header:
@@ -212,7 +83,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 1c - Pre Place
+  name: 3 Waypoint Pick Pose
 - description: ''
   favorite: false
   joint_group_names:
@@ -236,14 +107,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639636129146917
-    - -0.14375285311965005
-    - -2.8600433649921495
-    - -1.9461602101793198
-    - -1.8756681071759462
-    - -2.597844115240611
-    - -1.272458370221303
-    - 3.209846268837379
+    - 0.6020243803964747
+    - -0.5137181686901243
+    - -0.2723648029022852
+    - -1.0052256415555374
+    - 1.8573472354338991
+    - -2.4941180496510045
+    - -1.53809881022558
+    - -0.5347223269502246
     velocity: []
   multi_dof_joint_state:
     header:
@@ -255,7 +126,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 1d - Place
+  name: 3 Waypoint Place Pose
 - description: ''
   favorite: false
   joint_group_names:
@@ -279,143 +150,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639644752524631
-    - 0.8130633024853088
-    - -2.726859433670606
-    - -1.8891510728754208
-    - -1.424665285030513
-    - -3.112945310941763
-    - -1.1412179091798282
-    - 3.2296261212002166
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: Flask 2a - Pre Pick
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.7639671534487822
-    - 0.7786119481205361
-    - -2.5914980762265736
-    - -2.386004216551112
-    - -1.882582057672306
-    - -2.1695739396137577
-    - -1.0073664867894194
-    - 3.25251851297434
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: Flask 2b - Pick
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.7639660563824677
-    - -0.2402305144870719
-    - -2.8832984482674955
-    - -1.8018009466305653
-    - -1.685247569441112
-    - -2.956647634694288
-    - -1.2898622616661428
-    - 3.2014481210116723
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: Flask 2c - Pre Place
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.7639675199131791
-    - -0.24086053204199623
-    - -2.876977231591623
-    - -1.939806049514917
-    - -1.8984021894234757
-    - -2.6063762242952886
-    - -1.2836686932398813
-    - 3.202498456119905
+    - 0.06656709465883658
+    - 0.31742962464478663
+    - -0.07518824419393247
+    - -0.7207568718596512
+    - 0.8665668564352057
+    - -1.6837956156664982
+    - -1.5707507158288945
+    - -0.07517349313281775
     velocity: []
   multi_dof_joint_state:
     header:
@@ -427,7 +169,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 2d - Place
+  name: Above Pick Object
 - description: ''
   favorite: false
   joint_group_names:
@@ -451,14 +193,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639685849147584
-    - 0.6446458760506458
-    - -2.7780078487258892
-    - -1.9155374976991195
-    - -1.3864706183602202
-    - -3.1216050652579663
-    - -1.191550429146523
-    - 3.221797617570794
+    - 0.447969045650818
+    - 0.6474400118806204
+    - -0.06033511474114565
+    - -1.2791772736274891
+    - 2.3829614234150545
+    - -2.8734344275394026
+    - -1.5569395003854831
+    - -0.059925837606245605
     velocity: []
   multi_dof_joint_state:
     header:
@@ -470,7 +212,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 3a - Pre Pick
+  name: Flask 1 - Pick
 - description: ''
   favorite: false
   joint_group_names:
@@ -494,14 +236,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639682422101212
-    - 0.6144316834131173
-    - -2.6658657366983283
-    - -2.367501290161342
-    - -1.887225784038829
-    - -2.176385304935698
-    - -1.0805693269230385
-    - 3.239557435454622
+    - 0.47520773972835983
+    - -0.17547027905239
+    - -0.20267583823127783
+    - -1.2046710251373107
+    - 1.9032859450982782
+    - -2.4569993707131843
+    - -1.5292842611843418
+    - -0.19960068225580938
     velocity: []
   multi_dof_joint_state:
     header:
@@ -513,7 +255,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 3b - Pick
+  name: Flask 1 - Place
 - description: ''
   favorite: false
   joint_group_names:
@@ -537,14 +279,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639614914449789
-    - -0.24670890913790466
-    - -2.9097897371763177
-    - -2.057135798403447
-    - -1.2794632064231153
-    - -3.1064892677695757
-    - -1.3160558724133762
-    - 3.197049363506155
+    - 0.05255644806618728
+    - 0.8012030897493988
+    - -0.07032966095174241
+    - -1.2073944811122455
+    - 2.183090332371707
+    - -2.7463093672335517
+    - -1.5551496517533123
+    - -0.06950241310132783
     velocity: []
   multi_dof_joint_state:
     header:
@@ -556,7 +298,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 3c - Pre Place
+  name: Flask 2 - Pick
 - description: ''
   favorite: false
   joint_group_names:
@@ -580,14 +322,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639590920670264
-    - -0.25674010578815215
-    - -2.904024374297564
-    - -2.1809624103341
-    - -1.4905916971704432
-    - -2.771777026484768
-    - -1.3103568767884126
-    - 3.198001370172111
+    - 0.5536302060922608
+    - -0.23131956337150142
+    - -0.2893885581395494
+    - -1.2116786062171627
+    - 1.8954143536642922
+    - -2.4344194461107844
+    - -1.5133904193700953
+    - -0.2845874079784073
     velocity: []
   multi_dof_joint_state:
     header:
@@ -599,7 +341,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 3d - Place
+  name: Flask 2 - Place
 - description: ''
   favorite: true
   joint_group_names:
@@ -623,14 +365,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.01688327588273299
-    - -7.398158482916995e-11
-    - -3.8075799469380365e-09
-    - -1.8849146307875049
-    - 1.1008590123422952
-    - -1.256467912694488
-    - -1.5707500007156492
-    - 2.510085262493143e-12
+    - 0.020668448949024514
+    - 0.20879448072947002
+    - 0.048630656629801594
+    - -2.062190505651845
+    - 1.3002462715341856
+    - -1.278104242210415
+    - -1.592754310292377
+    - 0.043377416825533394
     velocity: []
   multi_dof_joint_state:
     header:
@@ -924,14 +666,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.36093425387337075
-    - 0.3468469487674742
-    - -0.08122232656648681
-    - -1.1118986430431266
-    - 0.7735698103006698
-    - -1.2414135949062157
-    - -1.5575126426436692
-    - -0.0803135811027271
+    - 0.05510852026671219
+    - 0.28444944718193443
+    - 0.014734089293982098
+    - -1.363461962694742
+    - 1.1221354194996502
+    - -1.3396454745687125
+    - -1.5584288659596743
+    - 0.015645930749496093
     velocity: []
   multi_dof_joint_state:
     header:
@@ -987,92 +729,6 @@
     twist: []
     wrench: []
   name: Park Far Right
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.06656709575770764
-    - 0.2544520984236789
-    - 0.00297097958947393
-    - -0.5454836524482858
-    - 1.028930149174636
-    - -2.02088483587436
-    - -1.5709145918870968
-    - 0.002734726536987722
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: Pick Cube
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.06656710453529632
-    - -0.026311129005491703
-    - 0.391188076851874
-    - -0.6421557305136849
-    - 1.2200227002982904
-    - -2.1172063445999307
-    - -1.5707036597762196
-    - 0.39119709035689026
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: Place Cube
 - description: Location for the robot to go to before placing bottle.
   favorite: false
   joint_group_names:
@@ -1096,14 +752,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.005274374788506071
-    - -0.65899502183384
-    - -0.011044571105883248
-    - -0.9740961688696632
-    - 1.2354891461984063
-    - -2.0150613197113008
-    - -1.5477140380091456
-    - -0.0030875579024600766
+    - 0.05510852026866443
+    - -0.7189055036814227
+    - 0.19457050986833238
+    - -2.087568874212717
+    - 1.7974325760598713
+    - -1.6887096601663198
+    - -1.627206520781829
+    - 0.19188249774293678
     velocity: []
   multi_dof_joint_state:
     header:
@@ -1288,89 +944,3 @@
     twist: []
     wrench: []
   name: Wrist 2 Min
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.01575741574061428
-    - 0.8900780780594006
-    - -2.802295813677834
-    - -2.68730855580549
-    - -1.2398951805562326
-    - -2.462048876440746
-    - -1.2180411722914284
-    - 3.2264553538354073
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: behind_flask
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.01434864013918258
-    - 0.6726165309151089
-    - 0.12515773780769576
-    - -0.41585738104189296
-    - 0.7247962836696739
-    - -2.9740281980535777
-    - -1.63154143414899
-    - 0.06364200257035163
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: near_rack
diff --git a/src/lab_sim_behaviors/CMakeLists.txt b/src/lab_sim_behaviors/CMakeLists.txt
new file mode 100644
index 00000000..0558bd92
--- /dev/null
+++ b/src/lab_sim_behaviors/CMakeLists.txt
@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 3.22)
+project(lab_sim_behaviors CXX)
+
+find_package(moveit_studio_common REQUIRED)
+moveit_studio_package()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  moveit_pro_behavior
+  moveit_pro_behavior_interface
+  moveit_studio_vision
+  moveit_studio_vision_msgs
+  pluginlib)
+foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${package} REQUIRED)
+endforeach()
+
+add_library(
+  lab_sim_behaviors
+  SHARED
+  src/compute_tray_place_positions_using_apriltags.cpp
+  src/register_behaviors.cpp)
+target_include_directories(
+  lab_sim_behaviors
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(lab_sim_behaviors ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+install(
+  TARGETS lab_sim_behaviors
+  EXPORT lab_sim_behaviorsTargets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES
+  DESTINATION include)
+
+if(BUILD_TESTING)
+  moveit_pro_behavior_test(lab_sim_behaviors)
+endif()
+
+pluginlib_export_plugin_description_file(
+  moveit_pro_behavior_interface lab_sim_behaviors_plugin_description.xml)
+
+ament_export_targets(lab_sim_behaviorsTargets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()
diff --git a/src/lab_sim_behaviors/include/lab_sim_behaviors/compute_tray_place_positions_using_apriltags.hpp b/src/lab_sim_behaviors/include/lab_sim_behaviors/compute_tray_place_positions_using_apriltags.hpp
new file mode 100644
index 00000000..84999b08
--- /dev/null
+++ b/src/lab_sim_behaviors/include/lab_sim_behaviors/compute_tray_place_positions_using_apriltags.hpp
@@ -0,0 +1,62 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#pragma once
+
+#include <behaviortree_cpp/action_node.h>
+#include <moveit_pro_behavior/behaviors/visualization/ros_publisher_handle.hpp>
+#include <moveit_pro_behavior_interface/behavior_context.hpp>
+#include <moveit_pro_behavior_interface/shared_resources_node.hpp>
+
+namespace lab_sim_behaviors
+{
+/**
+ * @brief Computes a grid of place positions within a tray detected via its AprilTag.
+ *
+ * @details Given an array of AprilTag detections and camera intrinsics, this Behavior finds the
+ * tray's AprilTag by ID, uses its 3D pose plus a known offset to locate the tray center, then
+ * generates a grid of place positions (multiple columns per row). Rows fill from the far end
+ * of the tray toward the tag (top-to-bottom). Positions are transformed from the camera frame
+ * to the world frame with a fixed gripper-down orientation.
+ *
+ * If an input image is provided, the Behavior annotates it with green circles sized to match
+ * the bottle diameter at each computed place position and publishes the result.
+ *
+ * | Data Port Name          | Port Type | Object Type                                         |
+ * | ----------------------- | --------- | --------------------------------------------------- |
+ * | detections              | input     | moveit_studio_vision_msgs::msg::ObjectDetectionArray |
+ * | camera_info             | input     | sensor_msgs::msg::CameraInfo                        |
+ * | input_image             | input     | sensor_msgs::msg::Image (optional)                  |
+ * | tray_apriltag_id        | input     | int                                                 |
+ * | num_rows                | input     | int                                                 |
+ * | columns_per_row         | input     | int                                                 |
+ * | row_spacing             | input     | double                                              |
+ * | column_spacing          | input     | double                                              |
+ * | tag_to_tray_center      | input     | double                                              |
+ * | bottle_diameter         | input     | double                                              |
+ * | visualization_topic     | input     | std::string                                         |
+ * | place_positions         | output    | std::vector<geometry_msgs::msg::PoseStamped>        |
+ */
+class ComputeTrayPlacePositionsUsingAprilTags final
+  : public moveit_pro::behaviors::SharedResourcesNode<BT::SyncActionNode>
+{
+public:
+  ComputeTrayPlacePositionsUsingAprilTags(
+      const std::string& name, const BT::NodeConfiguration& config,
+      const std::shared_ptr<moveit_pro::behaviors::BehaviorContext>& shared_resources,
+      std::unique_ptr<moveit_pro::behaviors::ROSPublisherHandle> ros_publisher_interface =
+          std::make_unique<moveit_pro::behaviors::ROSPublisherHandle>());
+
+  static BT::PortsList providedPorts();
+
+  static BT::KeyValueVector metadata();
+
+  BT::NodeStatus tick() override;
+
+private:
+  std::unique_ptr<moveit_pro::behaviors::ROSPublisherHandle> ros_publisher_interface_;
+};
+}  // namespace lab_sim_behaviors
diff --git a/src/lab_sim_behaviors/lab_sim_behaviors_plugin_description.xml b/src/lab_sim_behaviors/lab_sim_behaviors_plugin_description.xml
new file mode 100644
index 00000000..bbfc828d
--- /dev/null
+++ b/src/lab_sim_behaviors/lab_sim_behaviors_plugin_description.xml
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<library path="lab_sim_behaviors">
+  <class
+    type="lab_sim_behaviors::LabSimBehaviorsLoader"
+    base_class_type="moveit_pro::behaviors::SharedResourcesNodeLoaderBase"
+  />
+</library>
diff --git a/src/lab_sim_behaviors/package.xml b/src/lab_sim_behaviors/package.xml
new file mode 100644
index 00000000..c06505fe
--- /dev/null
+++ b/src/lab_sim_behaviors/package.xml
@@ -0,0 +1,30 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<package format="3">
+  <name>lab_sim_behaviors</name>
+  <version>6.5.0</version>
+  <description>Lab sim specific behavior plugins for MoveIt Pro</description>
+
+  <maintainer email="support@picknik.ai">MoveIt Pro Maintainer</maintainer>
+  <author email="support@picknik.ai">MoveIt Pro Maintainer</author>
+
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>moveit_studio_common</build_depend>
+
+  <depend>moveit_pro_behavior</depend>
+  <depend>moveit_pro_behavior_interface</depend>
+  <depend>moveit_studio_vision</depend>
+  <depend>moveit_studio_vision_msgs</depend>
+  <depend>pluginlib</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_clang_format</test_depend>
+  <test_depend>ament_clang_tidy</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/lab_sim_behaviors/src/compute_tray_place_positions_using_apriltags.cpp b/src/lab_sim_behaviors/src/compute_tray_place_positions_using_apriltags.cpp
new file mode 100644
index 00000000..1b8ca52a
--- /dev/null
+++ b/src/lab_sim_behaviors/src/compute_tray_place_positions_using_apriltags.cpp
@@ -0,0 +1,272 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#include <cmath>
+#include <vector>
+
+#include <cv_bridge/cv_bridge.h>
+#include <fmt/format.h>
+#include <Eigen/Geometry>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <lab_sim_behaviors/compute_tray_place_positions_using_apriltags.hpp>
+#include <moveit_pro_behavior_interface/get_required_ports.hpp>
+#include <moveit_pro_behavior_interface/metadata_fields.hpp>
+#include <moveit_studio_vision/image_utils.hpp>
+#include <moveit_studio_vision_msgs/msg/object_detection_array.hpp>
+#include <opencv2/opencv.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+
+namespace
+{
+inline constexpr auto kDescriptionComputeTrayPlacePositionsUsingAprilTags = R"(
+                <p>
+                    Computes a grid of place positions within a tray detected by its AprilTag.
+                    Finds the tray tag by ID, uses its 3D pose plus a known offset to define the
+                    tray center, then generates a grid of positions with the specified number of
+                    columns per row and row spacing. Rows fill from near the tag toward
+                    the far end of the tray (top-to-bottom). Positions are transformed to the world frame with a
+                    fixed gripper-down orientation. If an input image is provided, annotates it
+                    with green circles matching the bottle diameter at each position.
+                </p>
+            )";
+
+constexpr auto kPortIDDetections = "detections";
+constexpr auto kPortIDCameraInfo = "camera_info";
+constexpr auto kPortIDInputImage = "input_image";
+constexpr auto kPortIDTrayApriltagId = "tray_apriltag_id";
+constexpr auto kPortIDNumRows = "num_rows";
+constexpr auto kPortIDColumnsPerRow = "columns_per_row";
+constexpr auto kPortIDRowSpacing = "row_spacing";
+constexpr auto kPortIDColumnSpacing = "column_spacing";
+constexpr auto kPortIDTagToTrayCenter = "tag_to_tray_center";
+constexpr auto kPortIDBottleDiameter = "bottle_diameter";
+constexpr auto kPortIDVisualizationTopic = "visualization_topic";
+constexpr auto kPortIDPlacePositions = "place_positions";
+constexpr auto kDefaultVisualizationTopic = "/apriltag_detections";
+constexpr auto kMarkerThickness = 2;
+}  // namespace
+
+namespace lab_sim_behaviors
+{
+ComputeTrayPlacePositionsUsingAprilTags::ComputeTrayPlacePositionsUsingAprilTags(
+    const std::string& name, const BT::NodeConfiguration& config,
+    const std::shared_ptr<moveit_pro::behaviors::BehaviorContext>& shared_resources,
+    std::unique_ptr<moveit_pro::behaviors::ROSPublisherHandle> ros_publisher_interface)
+  : SharedResourcesNode<BT::SyncActionNode>(name, config, shared_resources)
+  , ros_publisher_interface_{ std::move(ros_publisher_interface) }
+{
+}
+
+BT::PortsList ComputeTrayPlacePositionsUsingAprilTags::providedPorts()
+{
+  return {
+    BT::InputPort<moveit_studio_vision_msgs::msg::ObjectDetectionArray>(kPortIDDetections, "{detections}",
+                                                                        "Array of AprilTag detections from "
+                                                                        "DetectAprilTags. Must include the tray tag."),
+    BT::InputPort<sensor_msgs::msg::CameraInfo>(kPortIDCameraInfo, "{camera_info}",
+                                                "Camera intrinsics used to project 3D positions to pixel coordinates "
+                                                "for visualization."),
+    BT::InputPort<sensor_msgs::msg::Image>(
+        kPortIDInputImage, "", "Optional camera image to annotate with green circles at each computed place position."),
+    BT::InputPort<int>(kPortIDTrayApriltagId, 1,
+                       "AprilTag ID assigned to the tray. Used to find the tray among all detections."),
+    BT::InputPort<int>(kPortIDNumRows, 3, "Number of rows of place positions along the tray's Y axis."),
+    BT::InputPort<int>(kPortIDColumnsPerRow, 2, "Number of columns per row along the tray's X axis."),
+    BT::InputPort<double>(kPortIDRowSpacing, 0.055, "Distance in meters between rows along the tray's Y axis."),
+    BT::InputPort<double>(kPortIDColumnSpacing, 0.05, "Distance in meters between columns along the tray's X axis."),
+    BT::InputPort<double>(kPortIDTagToTrayCenter, 0.15,
+                          "Distance in meters from the tray tag to the tray center along the tray's +Y axis."),
+    BT::InputPort<double>(kPortIDBottleDiameter, 0.04,
+                          "Bottle diameter in meters. Used to size the visualization circles on the annotated image."),
+    BT::InputPort<std::string>(kPortIDVisualizationTopic, kDefaultVisualizationTopic,
+                               "ROS image topic to publish the annotated image to. Only used when input_image is "
+                               "provided."),
+    BT::OutputPort<std::vector<geometry_msgs::msg::PoseStamped>>(kPortIDPlacePositions, "{place_positions}",
+                                                                 "Computed world-frame poses for placing objects in "
+                                                                 "the tray, with gripper-down orientation."),
+  };
+}
+
+BT::KeyValueVector ComputeTrayPlacePositionsUsingAprilTags::metadata()
+{
+  return { { moveit_pro::behaviors::kSubcategoryMetadataKey, "Vision" },
+           { moveit_pro::behaviors::kDescriptionMetadataKey, kDescriptionComputeTrayPlacePositionsUsingAprilTags } };
+}
+
+BT::NodeStatus ComputeTrayPlacePositionsUsingAprilTags::tick()
+{
+  const auto ports = moveit_pro::behaviors::getRequiredInputs(
+      getInput<moveit_studio_vision_msgs::msg::ObjectDetectionArray>(kPortIDDetections),
+      getInput<sensor_msgs::msg::CameraInfo>(kPortIDCameraInfo));
+
+  if (!ports.has_value())
+  {
+    shared_resources_->logger->publishFailureMessage(name(), "Failed to get required values from input data ports: " +
+                                                                 ports.error());
+    return BT::NodeStatus::FAILURE;
+  }
+
+  const auto& [detections_array, camera_info] = ports.value();
+
+  const int tray_apriltag_id = getInput<int>(kPortIDTrayApriltagId).value();
+  const int num_rows = getInput<int>(kPortIDNumRows).value();
+  const int columns_per_row = getInput<int>(kPortIDColumnsPerRow).value();
+  const double row_spacing = getInput<double>(kPortIDRowSpacing).value();
+  const double column_spacing = getInput<double>(kPortIDColumnSpacing).value();
+  const double tag_to_tray_center = getInput<double>(kPortIDTagToTrayCenter).value();
+  const double bottle_diameter = getInput<double>(kPortIDBottleDiameter).value();
+
+  // Find the tray AprilTag by matching tray_apriltag_id
+  const auto tray_it =
+      std::find_if(detections_array.detections.begin(), detections_array.detections.end(),
+                   [tray_apriltag_id](const auto& detection) { return detection.id == tray_apriltag_id; });
+
+  if (tray_it == detections_array.detections.end())
+  {
+    shared_resources_->logger->publishFailureMessage(
+        name(), fmt::format("Tray AprilTag with ID {} not found in detections.", tray_apriltag_id));
+    return BT::NodeStatus::FAILURE;
+  }
+
+  const auto& tray_pose = tray_it->pose.pose;
+  const std::string& camera_frame = tray_it->pose.header.frame_id;
+
+  // Get the tray tag's 3D pose in camera frame
+  const Eigen::Quaterniond tag_rotation(tray_pose.orientation.w, tray_pose.orientation.x, tray_pose.orientation.y,
+                                        tray_pose.orientation.z);
+  const Eigen::Vector3d tag_position(tray_pose.position.x, tray_pose.position.y, tray_pose.position.z);
+
+  // Define tray local axes in camera frame
+  const Eigen::Vector3d tray_x = tag_rotation * Eigen::Vector3d::UnitX();
+  const Eigen::Vector3d tray_y = tag_rotation * Eigen::Vector3d::UnitY();
+
+  // Compute tray center from tag position. The tag's +Y points away from the tray
+  // interior, so we move in the -Y direction to reach the tray center.
+  const Eigen::Vector3d tray_center = tag_position - tray_y * tag_to_tray_center;
+
+  // Generate grid positions. Rows fill from near the tag (-Y) toward the far end (+Y).
+  // Within each row, columns are centered around X=0.
+  const int num_positions = num_rows * columns_per_row;
+  std::vector<Eigen::Vector3d> positions_camera;
+  positions_camera.reserve(num_positions);
+
+  for (int row = 0; row < num_rows; ++row)
+  {
+    // Row 0 is closest to the tag (-Y), last row is at the far end (+Y)
+    const double row_offset = (row - (num_rows - 1) / 2.0) * row_spacing;
+
+    for (int col = 0; col < columns_per_row; ++col)
+    {
+      const double col_offset = (col - (columns_per_row - 1) / 2.0) * column_spacing;
+      positions_camera.push_back(tray_center + tray_y * row_offset + tray_x * col_offset);
+    }
+  }
+
+  // Look up camera_frame -> "world" transform
+  if (!shared_resources_->transform_buffer_ptr->canTransform("world", camera_frame, tf2::TimePointZero))
+  {
+    shared_resources_->logger->publishFailureMessage(name(), fmt::format("Cannot transform between '{}' and 'world'.",
+                                                                         camera_frame));
+    return BT::NodeStatus::FAILURE;
+  }
+  const geometry_msgs::msg::TransformStamped camera_to_world_tf =
+      shared_resources_->transform_buffer_ptr->lookupTransform("world", camera_frame, tf2::TimePointZero);
+  const Eigen::Isometry3d camera_to_world = tf2::transformToEigen(camera_to_world_tf);
+
+  // Transform all positions to world frame with gripper-down orientation (x=1, y=0, z=0, w=0)
+  std::vector<geometry_msgs::msg::PoseStamped> place_positions;
+  place_positions.reserve(num_positions);
+  for (const auto& pos_camera : positions_camera)
+  {
+    const Eigen::Vector3d pos_world = camera_to_world * pos_camera;
+
+    geometry_msgs::msg::PoseStamped pose;
+    pose.header.frame_id = "world";
+    pose.pose.position.x = pos_world.x();
+    pose.pose.position.y = pos_world.y();
+    pose.pose.position.z = pos_world.z();
+    pose.pose.orientation.x = 1.0;
+    pose.pose.orientation.y = 0.0;
+    pose.pose.orientation.z = 0.0;
+    pose.pose.orientation.w = 0.0;
+    place_positions.push_back(pose);
+  }
+
+  // Visualization: annotate the camera image with place position markers
+  const auto maybe_image = getInput<sensor_msgs::msg::Image>(kPortIDInputImage);
+  if (!maybe_image.has_value())
+  {
+    shared_resources_->logger->publishInfoMessage(name(), "No input_image provided; skipping visualization.");
+  }
+  else
+  {
+    try
+    {
+      const auto viz_topic = getInput<std::string>(kPortIDVisualizationTopic).value();
+      ros_publisher_interface_->init(viz_topic, shared_resources_);
+
+      const double fx = camera_info.k[0];
+      const double fy = camera_info.k[4];
+      const double cx = camera_info.k[2];
+      const double cy = camera_info.k[5];
+
+      const cv_bridge::CvImagePtr cv_image_ptr =
+          cv_bridge::toCvCopy(maybe_image.value(), sensor_msgs::image_encodings::RGB8);
+      cv::Mat& cv_image = cv_image_ptr->image;
+
+      const cv::Scalar green(0, 255, 0);
+
+      for (std::size_t i = 0; i < positions_camera.size(); ++i)
+      {
+        const auto& p = positions_camera[i];
+        if (p.z() <= 0.0)
+        {
+          continue;
+        }
+
+        const int u = static_cast<int>(fx * p.x() / p.z() + cx);
+        const int v = static_cast<int>(fy * p.y() / p.z() + cy);
+
+        // Skip positions that project outside the image bounds
+        if (u < 0 || u >= cv_image.cols || v < 0 || v >= cv_image.rows)
+        {
+          continue;
+        }
+
+        // Compute pixel radius from bottle diameter using pinhole projection
+        const double radius_3d = bottle_diameter / 2.0;
+        const int pixel_radius = std::max(1, static_cast<int>(fx * radius_3d / p.z()));
+
+        // Draw a green circle matching the bottle diameter and a cross at each place position
+        cv::circle(cv_image, cv::Point(u, v), pixel_radius, green, kMarkerThickness);
+        cv::line(cv_image, cv::Point(u - pixel_radius, v), cv::Point(u + pixel_radius, v), green, kMarkerThickness);
+        cv::line(cv_image, cv::Point(u, v - pixel_radius), cv::Point(u, v + pixel_radius), green, kMarkerThickness);
+
+        const std::string label = fmt::format("P{}", i);
+        const cv::Rect bbox(u - pixel_radius, v - pixel_radius, pixel_radius * 2, pixel_radius * 2);
+        moveit_studio::vision::overlayText(label, bbox, green, cv_image);
+      }
+
+      sensor_msgs::msg::Image annotated_image;
+      cv_image_ptr->toImageMsg(annotated_image);
+      annotated_image.header = maybe_image.value().header;
+
+      ros_publisher_interface_->publish_image(annotated_image);
+    }
+    catch (const cv_bridge::Exception& e)
+    {
+      shared_resources_->logger->publishFailureMessage(name(), fmt::format("cv_bridge error during visualization: {}",
+                                                                           e.what()));
+    }
+  }
+
+  setOutput(kPortIDPlacePositions, place_positions);
+
+  return BT::NodeStatus::SUCCESS;
+}
+}  // namespace lab_sim_behaviors
diff --git a/src/lab_sim_behaviors/src/register_behaviors.cpp b/src/lab_sim_behaviors/src/register_behaviors.cpp
new file mode 100644
index 00000000..b68a72d4
--- /dev/null
+++ b/src/lab_sim_behaviors/src/register_behaviors.cpp
@@ -0,0 +1,29 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#include <behaviortree_cpp/bt_factory.h>
+#include <moveit_pro_behavior_interface/behavior_context.hpp>
+#include <moveit_pro_behavior_interface/shared_resources_node_loader.hpp>
+
+#include <lab_sim_behaviors/compute_tray_place_positions_using_apriltags.hpp>
+
+#include <pluginlib/class_list_macros.hpp>
+
+namespace lab_sim_behaviors
+{
+class LabSimBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase
+{
+public:
+  void registerBehaviors(BT::BehaviorTreeFactory& factory,
+                         const std::shared_ptr<moveit_pro::behaviors::BehaviorContext>& shared_resources) override
+  {
+    moveit_pro::behaviors::registerBehavior<ComputeTrayPlacePositionsUsingAprilTags>(
+        factory, "ComputeTrayPlacePositionsUsingAprilTags", shared_resources);
+  }
+};
+}  // namespace lab_sim_behaviors
+
+PLUGINLIB_EXPORT_CLASS(lab_sim_behaviors::LabSimBehaviorsLoader, moveit_pro::behaviors::SharedResourcesNodeLoaderBase);
diff --git a/src/lab_sim_behaviors/test/CMakeLists.txt b/src/lab_sim_behaviors/test/CMakeLists.txt
new file mode 100644
index 00000000..8db41205
--- /dev/null
+++ b/src/lab_sim_behaviors/test/CMakeLists.txt
@@ -0,0 +1,4 @@
+find_package(ament_cmake_gtest REQUIRED)
+
+ament_add_gtest(test_behavior_plugins test_behavior_plugins.cpp)
+ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS})
diff --git a/src/lab_sim_behaviors/test/test_behavior_plugins.cpp b/src/lab_sim_behaviors/test/test_behavior_plugins.cpp
new file mode 100644
index 00000000..741998f0
--- /dev/null
+++ b/src/lab_sim_behaviors/test/test_behavior_plugins.cpp
@@ -0,0 +1,42 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#include <gtest/gtest.h>
+
+#include <behaviortree_cpp/bt_factory.h>
+#include <moveit_pro_behavior_interface/shared_resources_node_loader.hpp>
+#include <pluginlib/class_loader.hpp>
+#include <rclcpp/node.hpp>
+
+/**
+ * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and
+ * instantiated by the behavior tree factory.
+ */
+TEST(BehaviorTests, test_load_behavior_plugins)
+{
+  pluginlib::ClassLoader<moveit_pro::behaviors::SharedResourcesNodeLoaderBase> class_loader(
+      "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase");
+
+  auto node = std::make_shared<rclcpp::Node>("test_node");
+  auto shared_resources = std::make_shared<moveit_pro::behaviors::BehaviorContext>(node);
+
+  BT::BehaviorTreeFactory factory;
+  {
+    auto plugin_instance = class_loader.createUniqueInstance("lab_sim_behaviors::LabSimBehaviorsLoader");
+    ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources));
+  }
+  // Test that ClassLoader is able to find and instantiate each Behavior using the package's plugin description info.
+  EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "ComputeTrayPlacePositionsUsingAprilTags",
+                                                    BT::NodeConfiguration()));
+}
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/src/picknik_accessories b/src/picknik_accessories
index 876d0d2a..f58d20d4 160000
--- a/src/picknik_accessories
+++ b/src/picknik_accessories
@@ -1 +1 @@
-Subproject commit 876d0d2a13cabce81fbac045631e61d445641728
+Subproject commit f58d20d4dd95309bc60758821c8f23ffc0b35b8f