diff --git a/src/picknik_ur_site_config/objectives/pick_object.xml b/src/picknik_ur_site_config/objectives/pick_object.xml
index 22a2f1b9..3a0ad2b5 100644
--- a/src/picknik_ur_site_config/objectives/pick_object.xml
+++ b/src/picknik_ur_site_config/objectives/pick_object.xml
@@ -1,25 +1,39 @@
-<?xml version="1.0"?>
+<?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick Object">
-    <BehaviorTree ID="Pick Object" _description="Pick up and lift a small object" _favorite="true">
-        <Control ID="Sequence" name="root">
-            <SubTree ID="Open Gripper"/>
-            <Control ID="Sequence">
-                <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Select the object you wish to pick" pixel_coords="{pixel_coords}"/>
-                <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
-                <Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{output_poses}"/>
-                <Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{output_poses}" adjusted_poses="{adjusted_poses}"/>
-            </Control>
-            <Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml" parameters="{parameters}"/>
-            <Action ID="InitializeMTCTask" task_id="pick_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{pick_object_task}"/>
-            <Action ID="SetupMTCCurrentState" task="{pick_object_task}"/>
-            <Decorator ID="ForEachPoseStamped" vector_in="{adjusted_poses}" out="{grasp_pose}">
-                <Control ID="Sequence">
-                    <Action ID="SetupMTCPickObject" grasp_pose="{grasp_pose}" task="{pick_object_task}" parameters="{parameters}"/>
-                    <Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
-                    <SubTree ID="Wait for Trajectory Approval if User Available" solution="{pick_object_solution}"/>
-                    <Action ID="ExecuteMTCTask" solution="{pick_object_solution}"/>
-                </Control>
-            </Decorator>
+  <BehaviorTree ID="Pick Object" _description="Pick up and lift a small object" _favorite="true">
+    <Control ID="Sequence" name="root">
+      <SubTree ID="Open Gripper"/>
+      <Control ID="Sequence">
+        <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Select the object you wish to pick" pixel_coords="{pixel_coords}"/>
+        <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" timeout_sec="5.000000"/>
+        <Decorator ID="ForceSuccess">
+          <Control ID="Sequence">
+            <Action ID="AlwaysFailure"/>
+            <Action ID="TransformPointCloudFrame" output_cloud="{point_cloud}" target_frame="wrist_mounted_camera_color_optical_frame" input_cloud="{point_cloud}"/>
+          </Control>
+        </Decorator>
+        <Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{output_poses}"/>
+        <Action ID="ResetPoseStampedVector" vector="{output_poses_transformed}"/>
+        <Decorator ID="ForEachPoseStamped" out="{pose}" vector_in="{output_poses}">
+          <Control ID="Sequence">
+            <Action ID="TransformPose" output_pose="{pose_transformed}" input_pose="{pose}" quaternion_xyzw="1;0;0;0" translation_xyz="0;0;0.12"/>
+            <Action ID="AddPoseStampedToVector" input="{pose_transformed}" vector="{output_poses_transformed}"/>
+          </Control>
+        </Decorator>
+        <Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{output_poses_transformed}" adjusted_poses="{adjusted_poses}"/>
+        <Action ID="ClearSnapshot"/>
+      </Control>
+      <Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml" parameters="{parameters}"/>
+      <Decorator ID="ForEachPoseStamped" vector_in="{adjusted_poses}" out="{grasp_pose}">
+        <Control ID="Sequence">
+          <Action ID="InitializeMTCTask" task_id="pick_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{pick_object_task}"/>
+          <Action ID="SetupMTCCurrentState" task="{pick_object_task}"/>
+          <Action ID="SetupMTCPickObject" grasp_pose="{grasp_pose}" task="{pick_object_task}" parameters="{parameters}"/>
+          <Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
+          <SubTree ID="Wait for Trajectory Approval if User Available" solution="{pick_object_solution}"/>
+          <Action ID="ExecuteMTCTask" solution="{pick_object_solution}"/>
         </Control>
-    </BehaviorTree>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_site_config/objectives/pick_object_config.yaml b/src/picknik_ur_site_config/objectives/pick_object_config.yaml
index 09ca7d6e..a909b4cd 100644
--- a/src/picknik_ur_site_config/objectives/pick_object_config.yaml
+++ b/src/picknik_ur_site_config/objectives/pick_object_config.yaml
@@ -1,11 +1,9 @@
 SetupMTCPickObject:
-  # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
-  world_frame_name: "world"
-  arm_group_name: "manipulator"
-  end_effector_group_name: "gripper"
-  end_effector_name: "moveit_ee"
-  hand_frame_name: "grasp_link"
-  end_effector_closed_pose_name: "close"
-
   approach_distance: 0.1
-  lift_distance: 0.1
+  arm_group_name: manipulator
+  end_effector_closed_pose_name: close
+  end_effector_group_name: gripper
+  end_effector_name: moveit_ee
+  hand_frame_name: robotiq_85_base_link
+  lift_distance: 0
+  world_frame_name: world