From c29143488bcb9192e17eb53b0c557e40f7b196ab Mon Sep 17 00:00:00 2001
From: Chance Cardona <41308677+chancecardona@users.noreply.github.com>
Date: Wed, 17 Apr 2024 09:20:04 -0600
Subject: [PATCH] Revert "Merge v5.0 to main (#259)"

This reverts commit f04ccbbbb885bb5fe5a3952722a8b4746c7c29bf.
---
 .../ros2_robotiq_gripper                      |   2 +-
 src/picknik_ur_base_config/config/config.yaml |   1 -
 .../control/picknik_ur.ros2_control.yaml      |  17 ---
 src/picknik_ur_base_config/package.xml        |   1 -
 .../objectives/tree_nodes_model.xml           |  25 ++++
 .../open_door_fixed_handle_MTC_config.yaml    |  16 +++
 .../open_door_lever_handle_MTC_config.yaml    |  23 ++++
 .../detect_door_graspable_object.xml          |  15 +--
 .../detect_fixed_handle_graspable_object.xml  |   7 +-
 .../objectives/detect_hinge_line_segment.xml  |   2 +-
 .../detect_lever_handle_graspable_object.xml  |  55 +++++++++
 .../detect_lever_handle_line_segment.xml      |  27 -----
 .../objectives/open_cabinet_door.xml          | 107 ++++++++----------
 .../open_door_fixed_handle_MTC_config.yaml    |  75 +-----------
 .../objectives/open_door_fixed_handle_ml.xml  |  16 +--
 .../open_door_lever_handle_MTC_config.yaml    |   5 +-
 .../objectives/open_door_lever_handle_ml.xml  |  10 +-
 .../objectives/open_lever_handle_door.xml     |  20 ++--
 .../objectives/push_button_ml.xml             |   4 +-
 .../objectives/sample_april_tag.xml           |   2 +-
 .../objectives/take_snapshot.xml              |   8 +-
 21 files changed, 210 insertions(+), 228 deletions(-)
 create mode 100644 src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
 create mode 100644 src/picknik_ur_gazebo_config/open_door_fixed_handle_MTC_config.yaml
 create mode 100644 src/picknik_ur_gazebo_config/open_door_lever_handle_MTC_config.yaml
 create mode 100644 src/picknik_ur_site_config/objectives/detect_lever_handle_graspable_object.xml
 delete mode 100644 src/picknik_ur_site_config/objectives/detect_lever_handle_line_segment.xml

diff --git a/src/external_dependencies/ros2_robotiq_gripper b/src/external_dependencies/ros2_robotiq_gripper
index b6136bdc..25c6e0f2 160000
--- a/src/external_dependencies/ros2_robotiq_gripper
+++ b/src/external_dependencies/ros2_robotiq_gripper
@@ -1 +1 @@
-Subproject commit b6136bdc866a929bfb096890ca61dde1335ffd30
+Subproject commit 25c6e0f2b67c9b4ccf06b72078105b6154583ac9
diff --git a/src/picknik_ur_base_config/config/config.yaml b/src/picknik_ur_base_config/config/config.yaml
index c1622af6..c451d0be 100644
--- a/src/picknik_ur_base_config/config/config.yaml
+++ b/src/picknik_ur_base_config/config/config.yaml
@@ -196,7 +196,6 @@ ros2_control:
     - "joint_trajectory_controller"
     - "admittance_controller_open_door"
     - "joint_trajectory_controller_chained_open_door"
-    - "joint_trajectory_admittance_controller"
   # Any controllers here will not be spawned by MoveIt Pro.
   # [Optional, default=[]]
   controllers_not_managed: []
diff --git a/src/picknik_ur_base_config/config/control/picknik_ur.ros2_control.yaml b/src/picknik_ur_base_config/config/control/picknik_ur.ros2_control.yaml
index 808cb977..3224e08f 100644
--- a/src/picknik_ur_base_config/config/control/picknik_ur.ros2_control.yaml
+++ b/src/picknik_ur_base_config/config/control/picknik_ur.ros2_control.yaml
@@ -19,8 +19,6 @@ controller_manager:
       type: admittance_controller/AdmittanceController
     joint_trajectory_controller_chained_open_door:
       type: joint_trajectory_controller/JointTrajectoryController
-    joint_trajectory_admittance_controller:
-      type: moveit_pro_controllers/JointTrajectoryAdmittanceController
 
 io_and_status_controller:
   ros__parameters:
@@ -260,18 +258,3 @@ admittance_controller_open_door:
 
     # general settings
     enable_parameter_update_without_reactivation: true
-
-joint_trajectory_admittance_controller:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-    base_frame: base_link
-    tip_frame: grasp_link
-    sensor_frame: tool0
-    ee_frame: grasp_link
-    ft_sensor_name: tcp_fts_sensor
diff --git a/src/picknik_ur_base_config/package.xml b/src/picknik_ur_base_config/package.xml
index 7af16e02..b1b0269d 100644
--- a/src/picknik_ur_base_config/package.xml
+++ b/src/picknik_ur_base_config/package.xml
@@ -14,7 +14,6 @@
   <exec_depend>admittance_controller</exec_depend>
   <exec_depend>kinematics_interface_kdl</exec_depend>
   <exec_depend>moveit_planners_stomp</exec_depend>
-  <exec_depend>moveit_pro_controllers</exec_depend>
   <exec_depend>moveit_ros_perception</exec_depend>
   <exec_depend>moveit_studio_agent</exec_depend>
   <exec_depend>moveit_studio_behavior</exec_depend>
diff --git a/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml b/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
new file mode 100644
index 00000000..5b594c79
--- /dev/null
+++ b/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
@@ -0,0 +1,25 @@
+<root>
+  <TreeNodesModel>
+    <SubTree ID="Sample April Tag">
+        <description>
+          <p>
+              Samples the pose of an AprilTag and returns the average measurement.
+          </p>
+        </description>
+        <input_port name="num_samples" default="1">The number of times to sample the pose.</input_port>
+        <input_port name="tag_id" default="0">The AprilTag ID to detect.</input_port>
+        <input_port name="apriltag_config" default="apriltag_detection_config.yaml">The configuration file for detecting AprilTags.</input_port>
+        <input_port name="max_distance" default="0.02">The maximum distance allow between the average pose and the new sample (Euclidean distance between pose positions).</input_port>
+        <input_port name="max_rotation" default="0.2">The maximum angle allow between the average pose and the new sample (shortest angle between two pose quaternions).</input_port>
+        <output_port name="avg_pose" default="{avg_pose}">The calculated average pose.</output_port>
+    </SubTree>
+    <SubTree ID="Pick Object">
+        <description>
+          <p>
+              Picks an object at the specified grasp pose.
+          </p>
+        </description>
+        <input_port name="grasp_pose" default="{grasp_pose}">The desired grasp pose.</input_port>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/picknik_ur_gazebo_config/open_door_fixed_handle_MTC_config.yaml b/src/picknik_ur_gazebo_config/open_door_fixed_handle_MTC_config.yaml
new file mode 100644
index 00000000..98b3353b
--- /dev/null
+++ b/src/picknik_ur_gazebo_config/open_door_fixed_handle_MTC_config.yaml
@@ -0,0 +1,16 @@
+SetupMTCGraspThenMoveAlongArcPull:
+  arm_group_name: "manipulator"
+  end_effector_group_name: "gripper"
+  end_effector_name: "moveit_ee"
+  ik_frame_name: "grasp_link"
+  end_effector_closed_pose_name: "close"
+  end_effector_open_pose_name: "open"
+
+  # List of controllers to use during the stages that interact with the door.
+  # TODO: this file currently only exists as gazebo does not have the door admittance controller. Remove if this becomes available.
+  move_along_arc_controllers: "/joint_trajectory_controller_chained_open_door /robotiq_gripper_controller"
+
+  approach_distance: 0.10   # meters
+  translation_distance: 0.0   # meters (0 means only move along arc here, no linear component)
+  rotation_distance: 55.0  # Degrees
+  use_circular_arc: true
diff --git a/src/picknik_ur_gazebo_config/open_door_lever_handle_MTC_config.yaml b/src/picknik_ur_gazebo_config/open_door_lever_handle_MTC_config.yaml
new file mode 100644
index 00000000..d14aa527
--- /dev/null
+++ b/src/picknik_ur_gazebo_config/open_door_lever_handle_MTC_config.yaml
@@ -0,0 +1,23 @@
+SetupMTCGraspAndTwistThenMoveAlongArcPush:
+  arm_group_name: "manipulator"
+  end_effector_group_name: "gripper"
+  ik_frame_name: "grasp_link"
+
+  # List of controllers to use during the stages that interact with the door.
+  # TODO: this file currently only exists as gazebo does not have the door admittance controller. Remove if this becomes available.
+  move_along_arc_controllers: "/joint_trajectory_controller_chained_open_door /robotiq_gripper_controller"
+
+  # The hand should be placed 75% of the way down the handle length when
+  # manipulating the handle. Valid value range [0, 1.0].
+  handle_grasp_offset_percent: 0.75
+  # The distance above the handle the robot should approach with before
+  # manipulating it.
+  above_handle_approach_distance: 0.03 # meters
+  # The distance the hand should move past the end of the handle to release it.
+  release_handle_distance: 0.03 # meters
+  # How many degrees to rotate the lever handle (use positive value for
+  # counter-clockwise rotation and negative value for clockwise rotation).
+  handle_twist_angle: 65.0  # degrees
+  approach_distance: 0.10   # meters
+  push_open_distance: 0.05  # meters
+  use_circular_arc: true
diff --git a/src/picknik_ur_site_config/objectives/detect_door_graspable_object.xml b/src/picknik_ur_site_config/objectives/detect_door_graspable_object.xml
index 4abdcd3a..12aa3ccb 100644
--- a/src/picknik_ur_site_config/objectives/detect_door_graspable_object.xml
+++ b/src/picknik_ur_site_config/objectives/detect_door_graspable_object.xml
@@ -1,7 +1,7 @@
-<?xml version='1.0' encoding='UTF-8'?>
+<?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4" main_tree_to_execute="Detect Door Graspable Object">
   <!-- ////////// -->
-  <BehaviorTree ID="Detect Door Graspable Object" _description="Segments out a door from an input depth image and attempts to fit an output GraspableObject.">
+  <BehaviorTree ID="Detect Door Graspable Object" _description="Detects a door grapsable object from a depth image and gets a GraspableObject.">
     <!-- TODO: need to be directly in front of door for this objective to work until GetGraspableObjectsFromMasks3D calculates the collision more robustly for non cuboid objects. -->
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Get the point cloud from the wrist camera -->
@@ -11,7 +11,7 @@
       <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/>
       <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
       <!-- Segment out 2D masks and then convert to 3D masks and visualize in UI -->
-      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0.05" max_relative_area="0.8" timeout_sec="15.0" masks2d="{masks2d}" valid_classes="{detection_classes}"/>
+      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0.05" max_relative_area="0.8" timeout_sec="10.0" masks2d="{masks2d}" valid_classes="{detection_classes}"/>
       <Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
       <Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
         <Control ID="Sequence">
@@ -19,15 +19,16 @@
           <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/>
         </Control>
       </Decorator>
-      <!-- Convert 3D mask into graspable object, but as we don't need it for MTC  -->
-      <Decorator ID="ForceSuccess">
-        <Action ID="GetGraspableObjectsFromMasks3D" point_cloud="{point_cloud}" masks3d="{masks3d}" base_frame="world" plane_inlier_threshold="0.01" minimum_face_area="0.000625" face_separation_threshold="0.02" graspable_objects="{door_objects}"/>
-      </Decorator>
+      <!-- Convert 3D mask into graspable object -->
+      <Action ID="GetGraspableObjectsFromMasks3D" point_cloud="{point_cloud}" masks3d="{masks3d}" base_frame="world" plane_inlier_threshold="0.01" minimum_face_area="0.000625" face_separation_threshold="0.02" graspable_objects="{door_objects}"/>
       <Decorator ID="ForceSuccess">
         <Decorator ID="ForEachGraspableObject" vector_in="{door_objects}" out="{door_object}">
           <Action ID="AlwaysFailure"/>
         </Decorator>
       </Decorator>
+      <!-- Add object to planning scene and return success  -->
+      <Action ID="ModifyObjectInPlanningScene" object="{door_object}" apply_planning_scene_service="/apply_planning_scene"/>
+      <Action ID="LogMessage" message="Found a GraspableObject for the door." log_level="info"/>
     </Control>
   </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_site_config/objectives/detect_fixed_handle_graspable_object.xml b/src/picknik_ur_site_config/objectives/detect_fixed_handle_graspable_object.xml
index 271a6da7..a6eeacfd 100644
--- a/src/picknik_ur_site_config/objectives/detect_fixed_handle_graspable_object.xml
+++ b/src/picknik_ur_site_config/objectives/detect_fixed_handle_graspable_object.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="Detect Fixed Handle Graspable Object">
   <!-- ////////// -->
   <BehaviorTree ID="Detect Fixed Handle Graspable Object" _description="Detects a fixed-handle graspable object from a depth image and returns a PoseStamped for the handle centroid.">
@@ -12,7 +12,7 @@
         <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
       </Control>
       <!-- Segment out 2D masks and then convert to 3D masks and visualize in UI -->
-      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="15.0" masks2d="{handle_masks2d}" valid_classes="pull handle"/>
+      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="10.0" masks2d="{handle_masks2d}" valid_classes="pull handle"/>
       <Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{handle_masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
       <Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
         <Control ID="Sequence">
@@ -27,9 +27,10 @@
           <Action ID="AlwaysFailure"/>
         </Decorator>
       </Decorator>
+      <!-- Add object to planning scene and return success  -->
+      <Action ID="ModifyObjectInPlanningScene" object="{handle_object}" apply_planning_scene_service="/apply_planning_scene"/>
       <Action ID="ExtractGraspableObjectPose" graspable_object="{handle_object}" pose="{grasp_pose}"/>
       <Action ID="LogMessage" message="Successfully extracted necessary subframes for the detected pull handle." log_level="info"/>
-      <Action ID="ModifyObjectInPlanningScene" object="{handle_object}" apply_planning_scene_service="/apply_planning_scene"/>
     </Control>
   </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_site_config/objectives/detect_hinge_line_segment.xml b/src/picknik_ur_site_config/objectives/detect_hinge_line_segment.xml
index fed6bf55..32fd1e9b 100644
--- a/src/picknik_ur_site_config/objectives/detect_hinge_line_segment.xml
+++ b/src/picknik_ur_site_config/objectives/detect_hinge_line_segment.xml
@@ -17,7 +17,7 @@
         </Decorator>
       </Decorator>
       <!-- fit 3D mask to a line segment and return the two end poses. -->
-      <Action ID="FitLineSegmentToMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" base_frame="world" max_distance_from_line_for_inlier="0.02" line_segment_poses="{line_segment_poses}"/>
+      <Action ID="FitLineSegmentToMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" base_frame="world" max_distance_from_line_for_inlier="0.02" line_segment_start_point="{line_segment_start_point}" line_segment_end_point="{line_segment_end_point}"/>
       <Action ID="LogMessage" message="Fit a line segment to the detected hinge successfully." log_level="info"/>
     </Control>
   </BehaviorTree>
diff --git a/src/picknik_ur_site_config/objectives/detect_lever_handle_graspable_object.xml b/src/picknik_ur_site_config/objectives/detect_lever_handle_graspable_object.xml
new file mode 100644
index 00000000..7952dc39
--- /dev/null
+++ b/src/picknik_ur_site_config/objectives/detect_lever_handle_graspable_object.xml
@@ -0,0 +1,55 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4" main_tree_to_execute="Detect Lever Handle Graspable Object">
+  <!-- ////////// -->
+  <BehaviorTree ID="Detect Lever Handle Graspable Object" _description="Detects a lever-handle graspable object from a depth image and returns 2 PoseStampeds to represent the handle.">
+    <!-- TODO: need to be directly in front of handle for this objective to work until GetGraspableObjectsFromMasks3D calculates the collision more robustly for non cuboid objects or shape completion is available. -->
+    <Control ID="Sequence" name="TopLevelSequence">
+      <!-- Get the point cloud from the wrist camera -->
+      <Control ID="Sequence" name="GetDepthImage">
+        <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
+        <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
+        <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
+        <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
+      </Control>
+      <!-- Segment out 2D masks and then convert to 3D masks and visualize in UI -->
+      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="10.0" masks2d="{handle_tip_masks2d}" valid_classes="lever_handle_tip"/>
+      <Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{handle_tip_masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
+      <Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
+        <Control ID="Sequence">
+          <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
+          <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/>
+        </Control>
+      </Decorator>
+      <!-- Convert 3D mask into graspable object -->
+      <Action ID="GetGraspableObjectsFromMasks3D" point_cloud="{point_cloud}" masks3d="{masks3d}" base_frame="world" plane_inlier_threshold="0.01" minimum_face_area="0.000625" face_separation_threshold="0.02" graspable_objects="{handle_tip_objects}"/>
+      <Decorator ID="ForceSuccess">
+        <Decorator ID="ForEachGraspableObject" vector_in="{handle_tip_objects}" out="{handle_tip_object}">
+          <Action ID="AlwaysFailure"/>
+        </Decorator>
+      </Decorator>
+      <!-- Add object to planning scene and extract its pose  -->
+      <Action ID="ModifyObjectInPlanningScene" object="{handle_tip_object}" apply_planning_scene_service="/apply_planning_scene"/>
+      <Action ID="ExtractGraspableObjectPose" graspable_object="{handle_tip_object}" pose="{handle_tip_pose}"/>
+      <!-- Segment out 2D masks and then convert to 3D masks and visualize in UI -->
+      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="10.0" masks2d="{handle_pivot_masks2d}" valid_classes="lever_handle_pivot"/>
+      <Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{handle_pivot_masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
+      <Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
+        <Control ID="Sequence">
+          <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
+          <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/>
+        </Control>
+      </Decorator>
+      <!-- Convert 3D mask into graspable object -->
+      <Action ID="GetGraspableObjectsFromMasks3D" point_cloud="{point_cloud}" masks3d="{masks3d}" base_frame="world" plane_inlier_threshold="0.01" minimum_face_area="0.000625" face_separation_threshold="0.02" graspable_objects="{handle_pivot_objects}"/>
+      <Decorator ID="ForceSuccess">
+        <Decorator ID="ForEachGraspableObject" vector_in="{handle_pivot_objects}" out="{handle_pivot_object}">
+          <Action ID="AlwaysFailure"/>
+        </Decorator>
+      </Decorator>
+      <!-- Add object to planning scene and extract its pose  -->
+      <Action ID="ModifyObjectInPlanningScene" object="{handle_pivot_object}" apply_planning_scene_service="/apply_planning_scene"/>
+      <Action ID="ExtractGraspableObjectPose" graspable_object="{handle_pivot_object}" pose="{handle_pivot_pose}"/>
+      <Action ID="LogMessage" message="Successfully extracted necessary subframes for the detected lever handle." log_level="info"/>
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/picknik_ur_site_config/objectives/detect_lever_handle_line_segment.xml b/src/picknik_ur_site_config/objectives/detect_lever_handle_line_segment.xml
deleted file mode 100644
index f36a3d94..00000000
--- a/src/picknik_ur_site_config/objectives/detect_lever_handle_line_segment.xml
+++ /dev/null
@@ -1,27 +0,0 @@
-<?xml version='1.0' encoding='UTF-8'?>
-<root BTCPP_format="4" main_tree_to_execute="Detect Lever Handle Line Segment">
-  <!-- ////////// -->
-  <BehaviorTree ID="Detect Lever Handle Line Segment" _description="Detects a lever-handle Line Segment from a depth image and returns 2 PoseStampeds to represent the handle.">
-    <Control ID="Sequence" name="TopLevelSequence">
-      <!-- Get the point cloud from the wrist camera -->
-      <Control ID="Sequence" name="GetDepthImage">
-        <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
-        <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
-        <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
-        <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
-      </Control>
-      <!-- Segment out 2D masks and then convert to 3D masks and visualize in UI -->
-      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="10.0" masks2d="{handle_masks2d}" valid_classes="handle"/>
-      <Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{handle_masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
-      <Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
-        <Control ID="Sequence">
-          <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
-          <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/>
-        </Control>
-      </Decorator>
-      <!-- Fit a line segment to the 3D mask -->
-      <Action ID="FitLineSegmentToMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" base_frame="world" max_distance_from_line_for_inlier="0.01" line_segment_poses="{handle_poses}"/>
-      <Action ID="LogMessage" message="Successfully extracted necessary subframes for the detected lever handle." log_level="info"/>
-    </Control>
-  </BehaviorTree>
-</root>
diff --git a/src/picknik_ur_site_config/objectives/open_cabinet_door.xml b/src/picknik_ur_site_config/objectives/open_cabinet_door.xml
index f512473b..e337aacc 100644
--- a/src/picknik_ur_site_config/objectives/open_cabinet_door.xml
+++ b/src/picknik_ur_site_config/objectives/open_cabinet_door.xml
@@ -1,62 +1,51 @@
-<?xml version='1.0' encoding='UTF-8'?>
+<?xml version="1.0"?>
 <root BTCPP_format="4" main_tree_to_execute="Open Cabinet Door">
-  <BehaviorTree ID="Open Cabinet Door" _description="Open a cabinet door by grasping and pulling its handle" _favorite="false">
-    <Control ID="Sequence" name="Setup">
-      <!--Load the parameters needed for this Objective-->
-      <Action ID="LoadObjectiveParameters" config_file_name="open_door_fixed_handle_MTC_config.yaml" parameters="{parameters}"/>
-      <!--Update the admittance controller and reset the FTS-->
-      <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
-      <SubTree ID="Update Admittance Controller" _collapsed="true"/>
-      <!--Always take a snapshot when starting this behavior-->
-      <SubTree ID="Take Snapshot" _collapsed="true" point_cloud="{point_cloud}"/>
-      <!--Get Poses from User-->
-      <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Select Door Handle" pixel_coords="{grasp_pixel_coords}"/>
-      <Decorator ID="Delay" delay_msec="2000">
-        <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="hinge_axis_start;hinge_axis_end" point_prompts="Select Start Point of Door Hinge;Select End Point of Door Hinge" pixel_coords="{hinge_pixel_coords}"/>
-      </Decorator>
-      <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" timeout_sec="5.000000"/>
-      <Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{grasp_pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{grasp_poses}"/>
-      <Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{hinge_pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{hinge_poses}"/>
-      <!--Set up Subframes for MTC-->
-      <Decorator ID="ForEachPoseStamped" vector_in="{grasp_poses}" out="{grasp_pose}">
-        <Action ID="GetMoveAlongArcSubframes" target_grasp_pose="{grasp_pose}" hinge_axis_poses="{hinge_poses}" move_along_arc_subframes="{move_along_arc_subframes}"/>
-      </Decorator>
-      <Action ID="CreateGraspableObject" object_id="cabinet_door" generate_top_face="true" generate_front_face="true" generate_side_faces="true" cuboid_object="{handle_object}" pose="{grasp_pose}" dy="0.100000" dx="0.100000" dz="0.100000"/>
-      <!--Add the subframes to the Object-->
-      <Decorator ID="ForEachObjectSubframe" vector_in="{move_along_arc_subframes}" out="{subframe}">
-        <Action ID="AddSubframeToObject" subframe="{subframe}" graspable_object="{handle_object}"/>
-      </Decorator>
-      <!--Open Gripper and begin MTC-->
-      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
-      <Control ID="Sequence" name="OpenDoorFixedHandleMTC">
-        <Action ID="InitializeMTCTask" task_id="open_door_fixed_handle_user" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{open_door_fixed_handle_task}"/>
-        <Action ID="SetupMTCCurrentState" task="{open_door_fixed_handle_task}"/>
-        <!--Open Gripper-->
-        <Action ID="SetupMTCMoveToNamedState" task="{open_door_fixed_handle_task}" planning_group_name="gripper" goal_state_name="open" use_all_planners="false"/>
-        <!--Approach Grasp Point-->
-        <Action ID="SetupMTCApproachGrasp" task="{open_door_fixed_handle_task}" monitored_stage="{monitored_stage}" target_object="{handle_object}" parameters="{parameters}"/>
-        <!--Generate Cuboid Grasps at Handle-->
-        <Action ID="SetupMTCGenerateCuboidGrasps" parameters="{parameters}" target_object="{handle_object}" monitored_stage="{monitored_stage}" task="{open_door_fixed_handle_task}"/>
-        <!--Close Gripper around handle? todo: break out of MoveAlongArcPull and handle collisions.-->
-        <!--<Action ID="SetupMTCMoveToNamedState" task="{open_door_fixed_handle_task}" planning_group_name="gripper" goal_state_name="close" use_all_planners="false"/>-->
-        <!--Close gripper (here for now due to collisions) and Move Along Arc-->
-        <Action ID="SetupMTCGraspThenMoveAlongArcPull" graspable_object="{handle_object}" parameters="{parameters}" task="{open_door_fixed_handle_task}"/>
-        <!--End of MTC task setup, now plan-->
-        <Action ID="PlanMTCTask" solution="{open_door_fixed_handle_solution}" task="{open_door_fixed_handle_task}"/>
-        <SubTree ID="Wait for Trajectory Approval if User Available" solution="{open_door_fixed_handle_solution}" _collapsed="false"/>
-        <!--Whether task execution succeeds or fails, reset the controllers to disable admittance control.-->
-        <Control ID="IfThenElse" name="execute_and_reset_controllers">
-          <!--IF task execution succeeds-->
-          <Action ID="ExecuteMTCTask" solution="{open_door_fixed_handle_solution}"/>
-          <!--THEN reset the controllers-->
-          <Action ID="ActivateControllers" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
-          <!--ELSE reset the controllers and return failure-->
-          <Control ID="Sequence">
-            <Action ID="ActivateControllers" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
-            <Action ID="AlwaysFailure"/>
-          </Control>
+    <BehaviorTree ID="Open Cabinet Door" _description="Open a cabinet door by grasping and pulling its handle">
+        <Control ID="Sequence" name="Setup">
+            <!-- Load the parameters needed for this Objective -->
+            <Action ID="LoadObjectiveParameters" config_file_name="open_door_fixed_handle_MTC_config.yaml" parameters="{parameters}"/>
+            <!-- Update the admittance controller and reset the FTS -->
+            <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
+            <SubTree ID="Update Admittance Controller" _collapsed="true"/>
+
+            <!-- Get Poses from User -->
+            <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Select Door Handle" pixel_coords="{grasp_pixel_coords}"/>
+            <Action ID="WaitForDuration" delay_duration="1"/>
+            <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="hinge_axis_start;hinge_axis_end" point_prompts="Select Start Point of Door Hinge;Select End Point of Door Hinge" pixel_coords="{hinge_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="{grasp_pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{grasp_poses}"/>
+            <Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{hinge_pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{hinge_poses}"/>
+
+            <!-- Set up Subframes for MTC -->
+            <Decorator ID="ForEachPoseStamped" vector_in="{grasp_poses}" out="{grasp_pose}">
+                <Action ID="GetMoveAlongArcSubframes" target_grasp_pose="{grasp_pose}" hinge_axis_poses="{hinge_poses}" move_along_arc_subframes="{move_along_arc_subframes}" />
+            </Decorator>
+            <Action ID="CreateGraspableObject" pose="{grasp_pose}" dx="0.1" dy="0.1" dz="0.1" object_id="cabinet_door" generate_top_face="true" generate_front_face="true" generate_side_faces="true" cuboid_object="{door_object}"/>
+            <!-- Add the subframes to the Object-->
+            <Decorator ID="ForEachObjectSubframe" vector_in="{move_along_arc_subframes}" out="{subframe}">
+              <Action ID="AddSubframeToObject" subframe="{subframe}" graspable_object="{door_object}"/>
+            </Decorator>
+            <!-- Open Gripper and begin MTC -->
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0" />
+            <Control ID="Sequence" name="OpenDoorFixedHandleMTC">
+                <Action ID="InitializeMTCTask" task_id="open_door_fixed_handle_user" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{open_door_fixed_handle_task}" />
+                <Action ID="SetupMTCCurrentState" task="{open_door_fixed_handle_task}" />
+                <Action ID="SetupMTCGraspThenMoveAlongArcPull" graspable_object="{door_object}" parameters="{parameters}" task="{open_door_fixed_handle_task}" />
+                <Action ID="PlanMTCTask" solution="{open_door_fixed_handle_solution}" task="{open_door_fixed_handle_task}" />
+                <SubTree ID="Wait for Trajectory Approval if User Available" solution="{open_door_fixed_handle_solution}"/>
+                <!-- Whether task execution succeeds or fails, reset the controllers to disable admittance control. -->
+                <Control ID="IfThenElse" name="execute_and_reset_controllers">
+                    <!-- IF task execution succeeds -->
+                    <Action ID="ExecuteMTCTask" solution="{open_door_fixed_handle_solution}" />
+                    <!-- THEN reset the controllers -->
+                    <Action ID="ActivateControllers" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" />
+                    <!-- ELSE reset the controllers and return failure -->
+                    <Control ID="Sequence">
+                        <Action ID="ActivateControllers" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" />
+                        <Action ID="AlwaysFailure" />
+                    </Control>
+                </Control>
+            </Control>
         </Control>
-      </Control>
-    </Control>
-  </BehaviorTree>
+    </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_site_config/objectives/open_door_fixed_handle_MTC_config.yaml b/src/picknik_ur_site_config/objectives/open_door_fixed_handle_MTC_config.yaml
index a62d052e..e9a87d13 100644
--- a/src/picknik_ur_site_config/objectives/open_door_fixed_handle_MTC_config.yaml
+++ b/src/picknik_ur_site_config/objectives/open_door_fixed_handle_MTC_config.yaml
@@ -9,80 +9,7 @@ SetupMTCGraspThenMoveAlongArcPull:
   # List of controllers to use during the stages that interact with the door.
   move_along_arc_controllers: "/joint_trajectory_controller_chained_open_door /admittance_controller_open_door /robotiq_gripper_controller"
 
-  # Approach parameters used for retract phase for now.
-  approach_min_distance: 0.10   # meters
-  approach_max_distance: 1.00   # meters
-
+  approach_distance: 0.10   # meters
   translation_distance: 0.0   # meters (0 means only move along arc here, no linear component)
   rotation_distance: 55.0  # Degrees
   use_circular_arc: true
-
-SetupMTCApproachGrasp:
-  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"
-  ui_grasp_link: "grasp_link"
-
-  approach_distance: 0.15
-
-  # Set to true to make the robot keep the object within view of its camera while reaching to grasp it
-  enforce_visibility_constraint: true
-  camera_optical_frame_id: "wrist_mounted_camera_color_optical_frame"
-  camera_field_of_view_angle: 1.0472  # radians: approx. 60 degrees, which is narrower than the 70 degree diagonal FOV of the D415 camera
-  sensor_z_offset: 0.02
-  target_diameter: 0.0  # Set to 0 to disable the visibility cone constraint -- it's not useful with a single-arm camera-in-hand configuration.
-
-  # Grasp candidate configs
-  grasp_candidate_config:
-    generate_x_axis_grasps: true
-    generate_y_axis_grasps: true
-    generate_z_axis_grasps: true
-
-    # Number of samples to generate for each quadrant of the cuboid.
-    samples_per_quadrant: 3
-
-SetupMTCGenerateCuboidGrasps:
-  end_effector_group_name: "gripper"
-  end_effector_name: "moveit_ee"
-  hand_frame_name: "grasp_link"
-  end_effector_closed_pose_name: "close"
-  ui_grasp_link: "grasp_link"
-
-  # Grasp candidate configs
-  grasp_candidate_config:
-    generate_x_axis_grasps: true
-    generate_y_axis_grasps: true
-    generate_z_axis_grasps: true
-
-    # Number of samples to generate for each quadrant of the cuboid.
-    samples_per_quadrant: 3
-
-  # Grasp data configs
-  grasp_data:
-    # See https://ros-planning.github.io/moveit_tutorials/_images/finger_gripper_explanation.jpg for description of the following parameters
-    # Note: these parameters are copied from Robotiq gripper without tweaking
-    # min/max values of the grasp depth range in meter.
-    # This should fit to the distance from finger tip to inner palm or the suction cup stroke
-    grasp_min_depth: 0.01 # minimum amount fingers must overlap object
-    grasp_max_depth: 0.035 # Maximum distance from tip of end effector inwards that an object can be for a grasp
-
-    # Maximum allowed finger width for a grasp.
-    # This value should be considerably smaller than max_finger_width
-    # to allow padded collision checks
-    max_grasp_width: 0.1
-
-    # The transform from the arm IK link to the grasp point to align it with the following convention.
-    #
-    # z-axis pointing toward object to grasp
-    # x-axis perpendicular to movement of grippers
-    # y-axis parallel to movement of grippers
-    eef_mount_to_tcp_transform:
-      x: 0.0
-      y: 0.0
-      z: -0.02
-      roll: 0.0
-      pitch: 0.0
-      yaw: 1.57
diff --git a/src/picknik_ur_site_config/objectives/open_door_fixed_handle_ml.xml b/src/picknik_ur_site_config/objectives/open_door_fixed_handle_ml.xml
index 0e1986f7..bc60b3ad 100644
--- a/src/picknik_ur_site_config/objectives/open_door_fixed_handle_ml.xml
+++ b/src/picknik_ur_site_config/objectives/open_door_fixed_handle_ml.xml
@@ -28,9 +28,9 @@
       </Control>
       <!-- Get the masks for the hinge subframes of the door GraspableObject -->
       <SubTree ID="Move to Waypoint" waypoint_name="Hinge View" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
-      <SubTree ID="Detect Hinge Line Segment" _collapsed="true" line_segment_poses="{line_segment_poses}"/>
+      <SubTree ID="Detect Hinge Line Segment" _collapsed="true" line_segment_start_point="{line_segment_start_point}" line_segment_end_point="{line_segment_end_point}"/>
       <!-- Helper behavior to convert poses into subframes that adhere to the expected axis convention -->
-      <Action ID="GetMoveAlongArcSubframes" target_grasp_pose="{grasp_pose}" hinge_axis_poses="{line_segment_poses}" move_along_arc_subframes="{move_along_arc_subframes}"/>
+      <Action ID="GetMoveAlongArcSubframes" target_grasp_pose="{grasp_pose}" hinge_axis_pose_start="{line_segment_start_point}" hinge_axis_pose_end="{line_segment_end_point}" move_along_arc_subframes="{move_along_arc_subframes}"/>
       <!-- Add the subframes to the Object-->
       <Decorator ID="ForEachObjectSubframe" vector_in="{move_along_arc_subframes}" out="{subframe}">
         <Action ID="AddSubframeToObject" subframe="{subframe}" graspable_object="{door_object}"/>
@@ -41,17 +41,7 @@
       <Control ID="Sequence" name="OpenDoorFixedHandleMTC">
         <Action ID="InitializeMTCTask" task_id="open_door_fixed_handle_ml" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{open_door_fixed_handle_task}"/>
         <Action ID="SetupMTCCurrentState" task="{open_door_fixed_handle_task}"/>
-        <!--Open Gripper-->
-        <Action ID="SetupMTCMoveToNamedState" task="{open_door_fixed_handle_task}" planning_group_name="gripper" goal_state_name="open" use_all_planners="false"/>
-        <!--Approach Grasp Point-->
-        <Action ID="SetupMTCApproachGrasp" task="{open_door_fixed_handle_task}" monitored_stage="{monitored_stage}" target_object="{handle_object}" parameters="{parameters}"/>
-        <!--Generate Cuboid Grasps at Handle-->
-        <Action ID="SetupMTCGenerateCuboidGrasps" parameters="{parameters}" target_object="{handle_object}" monitored_stage="{monitored_stage}" task="{open_door_fixed_handle_task}"/>
-        <!--Close Gripper around handle? todo: break out of MoveAlongArcPull and handle collisions.-->
-        <!--<Action ID="SetupMTCMoveToNamedState" task="{open_door_fixed_handle_task}" planning_group_name="gripper" goal_state_name="close" use_all_planners="false"/>-->
-        <!--Close gripper (here for now due to collisions) and Move Along Arc-->
-        <Action ID="SetupMTCGraspThenMoveAlongArcPull" graspable_object="{handle_object}" parameters="{parameters}" task="{open_door_fixed_handle_task}"/>
-        <!--End of MTC task setup, now plan-->
+        <Action ID="SetupMTCGraspThenMoveAlongArcPull" graspable_object="{door_object}" parameters="{parameters}" task="{open_door_fixed_handle_task}"/>
         <Action ID="PlanMTCTask" task="{open_door_fixed_handle_task}" solution="{open_door_fixed_handle_solution}"/>
         <SubTree ID="Wait for Trajectory Approval if User Available" solution="{open_door_fixed_handle_solution}"/>
         <Action ID="ExecuteMTCTask" solution="{open_door_fixed_handle_solution}"/>
diff --git a/src/picknik_ur_site_config/objectives/open_door_lever_handle_MTC_config.yaml b/src/picknik_ur_site_config/objectives/open_door_lever_handle_MTC_config.yaml
index f3549175..fb702278 100644
--- a/src/picknik_ur_site_config/objectives/open_door_lever_handle_MTC_config.yaml
+++ b/src/picknik_ur_site_config/objectives/open_door_lever_handle_MTC_config.yaml
@@ -16,8 +16,7 @@ SetupMTCGraspAndTwistThenMoveAlongArcPush:
   release_handle_distance: 0.03 # meters
   # How many degrees to rotate the lever handle (use positive value for
   # counter-clockwise rotation and negative value for clockwise rotation).
-  handle_twist_angle: 45.0  # degrees
-  approach_max_distance: 0.05   # meters
-  approach_min_distance: 0.01   # meters
+  handle_twist_angle: 65.0  # degrees
+  approach_distance: 0.10   # meters
   push_open_distance: 0.05  # meters
   use_circular_arc: true
diff --git a/src/picknik_ur_site_config/objectives/open_door_lever_handle_ml.xml b/src/picknik_ur_site_config/objectives/open_door_lever_handle_ml.xml
index f5e7e792..0cdde888 100644
--- a/src/picknik_ur_site_config/objectives/open_door_lever_handle_ml.xml
+++ b/src/picknik_ur_site_config/objectives/open_door_lever_handle_ml.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="Open Door Lever Handle ML">
   <!-- ////////// -->
   <BehaviorTree ID="Open Door Lever Handle ML" _description="Attempts to open a door with a lever handle using machine learning." _favorite="false" _hardcoded="true">
@@ -12,13 +12,15 @@
       <SubTree ID="Move to Waypoint" waypoint_name="Extended Right 2" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
       <SubTree ID="Clear Snapshot" _collapsed="true"/>
       <SubTree ID="Reset Planning Scene" _collapsed="true"/>
+
       <SubTree ID="Detect Door Graspable Object" _collapsed="true" door_object="{door_object}" detection_classes="door"/>
-      <SubTree ID="Detect Lever Handle Line Segment" _collapsed="true" handle_poses="{handle_poses}"/>
-      <Action ID="GetDoorHandle" handle_poses="{handle_poses}" point_cloud="{point_cloud}" minimum_door_handle_depth="0.02" target_output_frame_id="world" target_handle_length="{handle_length}" target_handle_pose="{handle_pose}" target_handle_z_offset="{handle_z_offset}"/>
+      <SubTree ID="Detect Lever Handle Graspable Object" _collapsed="true" handle_tip_pose="{handle_tip_pose}" handle_pivot_pose="{handle_pivot_pose}"/> 
+      <Action ID="GetDoorHandle" handle_pivot_pose="{handle_pivot_pose}" handle_tip_pose="{handle_tip_pose}" point_cloud="{point_cloud}" minimum_door_handle_depth="0.02" target_output_frame_id="world" target_handle_length="{handle_length}" target_handle_pose="{handle_pose}" target_handle_z_offset="{handle_z_offset}"/>
       <Action ID="GetGraspAndTwistSubframes" grasp_rotation_z_radians="1.5708" target_grasp_pose="{grasp_pose}" grasp_and_twist_subframes="{grasp_and_twist_subframes}"/>
       <Decorator ID="ForEachObjectSubframe" vector_in="{grasp_and_twist_subframes}" out="{subframe}">
         <Action ID="AddSubframeToObject" subframe="{subframe}" graspable_object="{door_object}"/>
-      </Decorator>
+      </Decorator> 
+      <Action ID="ModifyObjectInPlanningScene" object="{door_object}" apply_planning_scene_service="/apply_planning_scene"/>
       <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
       <Control ID="Sequence" name="OpenDoorLeverHandleMTC">
         <Action ID="InitializeMTCTask" task_id="open_door_lever_handle_ml" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{open_door_lever_handle_task}"/>
diff --git a/src/picknik_ur_site_config/objectives/open_lever_handle_door.xml b/src/picknik_ur_site_config/objectives/open_lever_handle_door.xml
index 4fc80654..664e7024 100644
--- a/src/picknik_ur_site_config/objectives/open_lever_handle_door.xml
+++ b/src/picknik_ur_site_config/objectives/open_lever_handle_door.xml
@@ -1,20 +1,20 @@
-<?xml version='1.0' encoding='UTF-8'?>
+<?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4" main_tree_to_execute="Open Lever Handle Door">
   <BehaviorTree ID="Open Lever Handle Door" _description="Open a lever handle door by turning the handle and pushing the door" _favorite="false">
     <Control ID="Sequence" name="Setup">
       <Action ID="LoadObjectiveParameters" config_file_name="open_door_lever_handle_MTC_config.yaml" parameters="{parameters}"/>
       <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
       <SubTree ID="Update Admittance Controller" _collapsed="true"/>
-      <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" timeout_sec="5.000000"/>
+      <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
       <Action ID="PublishPointCloud" point_cloud="{point_cloud}" point_cloud_topic="studio_vision_debug_snapshot"/>
       <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="handle_pivot; handle_tip" point_prompts="Click on center of axis of handle;Click on end of handle" pixel_coords="{pixel_coords}"/>
-      <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" timeout_sec="5.000000"/>
+      <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="{handle_poses}"/>
       <Action ID="GetDoorHandle" handle_poses="{handle_poses}" point_cloud="{point_cloud}" minimum_door_handle_depth="0.02" target_output_frame_id="world" target_handle_length="{handle_length}" target_handle_pose="{handle_pose}" target_handle_z_offset="{handle_z_offset}"/>
-      <!--Set up Subframes for MTC-->
+      <!-- Set up Subframes for MTC -->
       <Action ID="GetGraspAndTwistSubframes" grasp_rotation_z_radians="1.5708" target_grasp_pose="{handle_pose}" grasp_and_twist_subframes="{grasp_and_twist_subframes}"/>
-      <Action ID="CreateGraspableObject" cuboid_object="{door_object}" generate_side_faces="true" generate_top_face="true" dy="0.100000" dx="0.100000" generate_front_face="true" object_id="lever_door" dz="0.100000" pose="{handle_pose}"/>
-      <!--Add the subframes to the Object-->
+      <Action ID="CreateGraspableObject" px="0.0" py="0.0" pz="0.0" rx="0.0" ry="0.0" rz="0.0" dx="0.1" dy="0.1" dz="0.1" frame_id="world" object_id="lever_door" generate_top_face="true" generate_front_face="true" generate_side_faces="true" cuboid_object="{door_object}"/>
+      <!-- Add the subframes to the Object-->
       <Decorator ID="ForEachObjectSubframe" vector_in="{grasp_and_twist_subframes}" out="{subframe}">
         <Action ID="AddSubframeToObject" subframe="{subframe}" graspable_object="{door_object}"/>
       </Decorator>
@@ -25,13 +25,13 @@
         <Action ID="SetupMTCGraspAndTwistThenMoveAlongArcPush" graspable_object="{door_object}" handle_length="{handle_length}" handle_z_offset="{handle_z_offset}" parameters="{parameters}" task="{open_lever_handle_door_task}"/>
         <Action ID="PlanMTCTask" solution="{open_lever_handle_door_solution}" task="{open_lever_handle_door_task}"/>
         <SubTree ID="Wait for Trajectory Approval if User Available" solution="{open_lever_handle_door_solution}"/>
-        <!--Whether task execution succeeds or fails, reset the controllers to disable admittance control.-->
+        <!-- Whether task execution succeeds or fails, reset the controllers to disable admittance control. -->
         <Control ID="IfThenElse" name="execute_and_reset_controllers">
-          <!--IF task execution succeeds-->
+          <!-- IF task execution succeeds -->
           <Action ID="ExecuteMTCTask" solution="{open_lever_handle_door_solution}"/>
-          <!--THEN reset the controllers-->
+          <!-- THEN reset the controllers -->
           <Action ID="ActivateControllers" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
-          <!--ELSE reset the controllers and return failure-->
+          <!-- ELSE reset the controllers and return failure -->
           <Control ID="Sequence">
             <Action ID="ActivateControllers" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
             <Action ID="AlwaysFailure"/>
diff --git a/src/picknik_ur_site_config/objectives/push_button_ml.xml b/src/picknik_ur_site_config/objectives/push_button_ml.xml
index acbb1a99..44750831 100644
--- a/src/picknik_ur_site_config/objectives/push_button_ml.xml
+++ b/src/picknik_ur_site_config/objectives/push_button_ml.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="Push Button ML">
   <BehaviorTree ID="Push Button ML" _description="Move to a waypoint, and then push a button detected by a image segmentation network with the end effector" _favorite="false">
     <Control ID="Sequence">
@@ -11,7 +11,7 @@
       <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
       <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
       <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
-      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" valid_classes="button" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="0.7" timeout_sec="15" masks2d="{masks2d}"/>
+      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" valid_classes="button" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="0.7" timeout_sec="5" masks2d="{masks2d}"/>
       <Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
       <Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
         <Control ID="Sequence">
diff --git a/src/picknik_ur_site_config/objectives/sample_april_tag.xml b/src/picknik_ur_site_config/objectives/sample_april_tag.xml
index 81573035..110d83c6 100644
--- a/src/picknik_ur_site_config/objectives/sample_april_tag.xml
+++ b/src/picknik_ur_site_config/objectives/sample_april_tag.xml
@@ -5,7 +5,7 @@
       <Decorator ID="Repeat" num_cycles="{num_samples}" name="SamplePose">
         <Control ID="Sequence">
         <Action ID="LoadObjectiveParameters" config_file_name="{apriltag_config}" parameters="{parameters}"/>
-        <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
+        <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>         
         <Decorator ID="RetryUntilSuccessful" num_attempts="5">
           <Control ID="Sequence">                      
             <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
diff --git a/src/picknik_ur_site_config/objectives/take_snapshot.xml b/src/picknik_ur_site_config/objectives/take_snapshot.xml
index 8b898216..f3a8c4ca 100644
--- a/src/picknik_ur_site_config/objectives/take_snapshot.xml
+++ b/src/picknik_ur_site_config/objectives/take_snapshot.xml
@@ -1,14 +1,14 @@
-<?xml version='1.0' encoding='UTF-8'?>
+<?xml version="1.0"?>
 <root BTCPP_format="4" main_tree_to_execute="Take Snapshot">
-  <!--//////////-->
+  <!-- ////////// -->
   <BehaviorTree ID="Take Snapshot" _description="Take a point cloud snapshot and add to collision scene" _favorite="true">
     <Control ID="Sequence">
       <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
-      <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" timeout_sec="15.000000"/>
+      <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
       <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
       <Control ID="Fallback" name="TryGetStringFromUser">
         <Action ID="GetStringFromUser" parameter_name="take_snapshot.uuid" parameter_value="{uuid}"/>
-        <!--if we fail to get the string from the user fallback to an empty value for the uuid-->
+        <!-- if we fail to get the string from the user fallback to an empty value for the uuid -->
         <Action ID="Script" code="uuid := ''"/>
       </Control>
       <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures" point_cloud_uuid="{uuid}"/>