From 71b922bf533ec5b2ceba1b4156beeb29e3e80bfd Mon Sep 17 00:00:00 2001
From: Dave Coleman <dave@picknik.ai>
Date: Thu, 26 Feb 2026 22:30:02 -0700
Subject: [PATCH] Add lab desk objects, adjust collision wall/tray positions,
 and clean up lab_sim

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
---
 .../control/picknik_ur.ros2_control.yaml      |   11 +-
 src/lab_sim/description/keyboard-texture.png  |    3 +
 src/lab_sim/description/laptop-screen.png     |    3 +
 src/lab_sim/description/picknik_ur.xacro      |   12 +-
 src/lab_sim/description/scene.xml             | 1037 ++++++++++++++++-
 .../objectives/3_waypoint_pick_and_place.xml  |   18 +-
 .../constrained_pick_and_place_subtree.xml    |  327 ++++--
 .../objectives/constrained_pick_place.xml     |   53 -
 .../objectives/cycle_between_waypoints.xml    |    2 +-
 .../objectives/move_flasks_to_burners.xml     |   33 +
 ...tle.xml => pick_1_pill_bottle_with_ml.xml} |   14 +-
 .../pick_all_bottles_with_apriltags.xml       |  571 +++++++++
 .../test/objectives_integration_test.py       |   45 +-
 src/lab_sim/waypoints/ur_waypoints.yaml       |  622 ++--------
 src/picknik_accessories                       |    2 +-
 15 files changed, 2038 insertions(+), 715 deletions(-)
 create mode 100644 src/lab_sim/description/keyboard-texture.png
 create mode 100644 src/lab_sim/description/laptop-screen.png
 delete mode 100644 src/lab_sim/objectives/constrained_pick_place.xml
 create mode 100644 src/lab_sim/objectives/move_flasks_to_burners.xml
 rename src/lab_sim/objectives/{pick_1_pill_bottle.xml => pick_1_pill_bottle_with_ml.xml} (91%)
 create mode 100644 src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml

diff --git a/src/lab_sim/config/control/picknik_ur.ros2_control.yaml b/src/lab_sim/config/control/picknik_ur.ros2_control.yaml
index 022396adb..4638d9666 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 000000000..9e685eab3
--- /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 000000000..67982b9f1
--- /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 fe73e3e84..98d1a4342 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 7d896c290..40616afdb 100644
--- a/src/lab_sim/description/scene.xml
+++ b/src/lab_sim/description/scene.xml
@@ -6,14 +6,96 @@
 
   <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" />
+    <headlight ambient="0.3 0.3 0.3" diffuse="0.5 0.5 0.5" />
+  </visual>
+
   <worldbody>
-    <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
+    <light
+      pos="0 0 1.5"
+      dir="0 0 -1"
+      directional="true"
+      diffuse="1 1 1"
+      ambient="0.3 0.3 0.3"
+    />
 
     <light
       name="spotlight"
@@ -31,51 +113,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>
-    <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" />
+    <!-- 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>
-    <body name="block3" pos="-0.2 0.75 0.575">
-      <freejoint name="block3" />
+    <!-- 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.025 0.025 0.025"
-        rgba="1 1 1 1"
+        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>
+    <!-- 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>
+    <!-- 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.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 c0f1f9756..c4698d040 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/constrained_pick_and_place_subtree.xml b/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml
index 28df92cd3..6c704ee5e 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,276 @@
   <!--//////////-->
   <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)-->
+          <SubTree ID="Close Gripper" />
+        </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
deleted file mode 100644
index 0b435156e..000000000
--- a/src/lab_sim/objectives/constrained_pick_place.xml
+++ /dev/null
@@ -1,53 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Move Flasks to Burners">
-  <!--//////////-->
-  <BehaviorTree
-    ID="Move Flasks to Burners"
-    _description="Example of picking and placing objects with orientation constraints enforced on the arm's motion"
-    _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"
-        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"
-        name="Flask 2"
-        _collapsed="true"
-      />
-    </Control>
-  </BehaviorTree>
-  <TreeNodesModel>
-    <SubTree ID="Move Flasks to Burners">
-      <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-        <Metadata runnable="true" />
-      </MetadataFields>
-    </SubTree>
-  </TreeNodesModel>
-</root>
diff --git a/src/lab_sim/objectives/cycle_between_waypoints.xml b/src/lab_sim/objectives/cycle_between_waypoints.xml
index 477b9bcba..aea34e587 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/move_flasks_to_burners.xml b/src/lab_sim/objectives/move_flasks_to_burners.xml
new file mode 100644
index 000000000..6d9a23460
--- /dev/null
+++ b/src/lab_sim/objectives/move_flasks_to_burners.xml
@@ -0,0 +1,33 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Move Flasks to Burners">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Move Flasks to Burners"
+    _description="Example of picking and placing objects with orientation constraints and force-controlled placement"
+    _favorite="true"
+  >
+    <Control ID="Sequence">
+      <SubTree ID="Open Gripper" />
+      <SubTree
+        ID="Constrained Pick and Place Subtree"
+        pick="Flask 1 - Pick"
+        place="Flask 1 - Place"
+        name="Flask 1"
+      />
+      <SubTree
+        ID="Constrained Pick and Place Subtree"
+        pick="Flask 2 - Pick"
+        place="Flask 2 - Place"
+        name="Flask 2"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Move Flasks to Burners">
+      <MetadataFields>
+        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata runnable="true" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
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 19c048cf2..8d4e96bdf 100644
--- a/src/lab_sim/objectives/pick_1_pill_bottle.xml
+++ b/src/lab_sim/objectives/pick_1_pill_bottle_with_ml.xml
@@ -1,10 +1,10 @@
 <?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"
+    _favorite="false"
   >
     <Control ID="Sequence">
       <SubTree ID="Open Gripper" _collapsed="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 000000000..1af9da5aa
--- /dev/null
+++ b/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml
@@ -0,0 +1,571 @@
+<?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">
+      <!--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.83; 0.85"
+      />
+      <Action
+        ID="CreateGraspableObject"
+        cuboid_object="{back_wall_object}"
+        dx="2.0"
+        dy="0.05"
+        dz="1.0"
+        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.125"
+          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/test/objectives_integration_test.py b/src/lab_sim/test/objectives_integration_test.py
index db004f12f..2825aeb7d 100644
--- a/src/lab_sim/test/objectives_integration_test.py
+++ b/src/lab_sim/test/objectives_integration_test.py
@@ -40,46 +40,40 @@
 # Looping objectives to cancel partway through
 cancel_objectives = {
     "3 Waypoints Pick and Place",
-    "Stationary Admittance",
+    "Classical Pick and Place",
     "Cycle Between Waypoints",
-    "Grasp Planning",
+    "Get AprilTag Pose from Image",
     "Grasp Pose Tuning With April Tag",
     "Grasp Pose Using Yaml",
-    "Joint Diagnostic",
-    "Loop Detect AprilTag",
-    "Pick And Place Example",
     "Pick April Tag Labeled Object With Approval",
-    "Place Object",
     "Plan and Save Trajectory",
     "Record and Replay Scanning Motion",
-    "Classical Pick and Place",
-    "ML Grasp Object from Text Prompt",
+    "Stationary Admittance",
 }
 
 # Objectives to skip entirely from integration testing
 skip_objectives = {
-    "Grasp Object from Text Prompt",  # https://github.com/PickNikRobotics/moveit_pro/issues/13236
+    "AddBottlesToPlanningScene",
+    "Get Grasp from Text Prompt Subtree",  # https://github.com/PickNikRobotics/moveit_pro/issues/13236
     "Grasp Planning",
     "Joint Diagnostic",
-    "Scan Multiple Views",
-    "MPC Pose Tracking",
-    "MPC Pose Tracking With Point Cloud Avoidance",
+    "ML Auto Grasp Object from Clicked Point",  # Skipped because there is no primary ui to switch to in ci
     "ML Find Bottles on Table from Image Exemplar",  # Skipped because it looks for a file on a home path
     "ML Segment Bottles from File",  # Skipped because it looks for a file on a home path
     "ML Segment Image",
     "ML Segment Point Cloud from Clicked Point",
-    "ML Auto Grasp Object from Clicked Point",  # Skipped because there is no primary ui to switch to in ci
+    "MPC Pose Tracking",
+    "MPC Pose Tracking With Point Cloud Avoidance",
+    "Octomap Example",  # Requires user input to clear the octomap.
+    "Pick 1 Pill Bottle with ML",
+    "Pick All Bottles with AprilTags",
     "Pick All Pill Bottles",
-    "Pick up Cube",
+    "Pick up Object",
     "Place Object",
     "Record Square Trajectory",
-    "Stack Blocks with ICP",  # Skipped because there is no primary ui to switch to in ci
-    "Teleoperate",
+    "Scan Scene - Multiple Point Clouds",
+    "Stack Objects with ICP",  # Skipped because there is no primary ui to switch to in ci
     "Stitch Multiple Point Clouds Together",
-    "Pick 1 Pill Bottle",
-    "AddBottlesToPlanningScene",
-    "Marker Visualization Example",  # Server not available for GetTextFromUser
-    "Octomap Example",  # Requires user input to clear the octomap.
 }
 
 
@@ -106,4 +100,13 @@ def test_all_objectives(
     execute_objective_resource: ExecuteObjectiveResource,
 ):
     wait_for_gripper_action_server()
-    run_objective(objective_id, should_cancel, execute_objective_resource)
+    try:
+        run_objective(objective_id, should_cancel, execute_objective_resource)
+    except AssertionError as e:
+        mode = "cancel" if should_cancel else "execute"
+        pytest.fail(f"Objective '{objective_id}' failed to {mode}: {e}")
+    except Exception as e:
+        mode = "cancel" if should_cancel else "execute"
+        pytest.fail(
+            f"Objective '{objective_id}' hit an unexpected error during {mode}: {type(e).__name__}: {e}"
+        )
diff --git a/src/lab_sim/waypoints/ur_waypoints.yaml b/src/lab_sim/waypoints/ur_waypoints.yaml
index a74e2aa09..c857a2a97 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/picknik_accessories b/src/picknik_accessories
index 876d0d2a1..0e3a8aaa6 160000
--- a/src/picknik_accessories
+++ b/src/picknik_accessories
@@ -1 +1 @@
-Subproject commit 876d0d2a13cabce81fbac045631e61d445641728
+Subproject commit 0e3a8aaa668268c82e6ff2edbc84110404a2810f