diff --git a/src/lab_sim/objectives/compute_posestamped_from_waypoint.xml b/src/lab_sim/objectives/compute_posestamped_from_waypoint.xml
new file mode 100644
index 00000000..4d786f33
--- /dev/null
+++ b/src/lab_sim/objectives/compute_posestamped_from_waypoint.xml
@@ -0,0 +1,47 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="ComputePoseStampedFromWaypoint">
+  <!--//////////-->
+  <BehaviorTree ID="ComputePoseStampedFromWaypoint">
+    <Control ID="Sequence" _collapsed="false">
+      <Action
+        ID="RetrieveWaypoint"
+        joint_group_name="manipulator"
+        waypoint_joint_state="{target_joint_state}"
+        waypoint_name="Above Pick Cube"
+      />
+      <Action
+        ID="UnpackRobotJointStateMessage"
+        joint_state="{joint_state}"
+        multi_dof_joint_state="{multi_dof_joint_state}"
+        robot_joint_state="{target_joint_state}"
+      />
+      <Action
+        ID="GetCurrentPlanningScene"
+        planning_scene_msg="{planning_scene}"
+      />
+      <Action
+        ID="ComputeLinkPoseForwardKinematics"
+        joint_state="{joint_state}"
+        link_name="grasp_link"
+        link_pose="{link_pose}"
+        planning_scene="{planning_scene}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="ComputePoseStampedFromWaypoint">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata
+          description="This objective retrieves a named waypoint, unpacks the joint state, gets the current planning scene, and computes the pose of a specified link using forward kinematics. The resulting pose is output as a geometry_msgs::msg::PoseStamped."
+        />
+      </MetadataFields>
+      <inout_port
+        name="link_pose"
+        default="{link_pose}"
+        type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"
+      />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/create_pcl_poses_vector.xml b/src/lab_sim/objectives/create_pcl_poses_vector.xml
new file mode 100644
index 00000000..a1df1862
--- /dev/null
+++ b/src/lab_sim/objectives/create_pcl_poses_vector.xml
@@ -0,0 +1,94 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Create PCL Poses Vector">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Create PCL Poses Vector"
+    _favorite="false"
+    _description="Creates a sequence of stamped poses and adds them to a vector"
+    target_poses="{target_poses}"
+  >
+    <Control ID="Sequence">
+      <Action
+        ID="CreateStampedPose"
+        orientation_xyzw="0.000; 0.000; 0.000; 1.000"
+        position_xyz="0.5;0.5;0.9"
+        stamped_pose="{stamped_pose}"
+        reference_frame="world"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        vector="{target_poses}"
+        input="{stamped_pose}"
+      />
+      <Action
+        ID="CreateStampedPose"
+        orientation_xyzw="0.000; 0.000; 0.000; 1.000"
+        position_xyz="0.4;0.5;0.9"
+        stamped_pose="{stamped_pose}"
+        reference_frame="world"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        vector="{target_poses}"
+        input="{stamped_pose}"
+      />
+      <Action
+        ID="CreateStampedPose"
+        orientation_xyzw="0.000; 0.000; 0.000; 1.000"
+        position_xyz="0.3;0.5;0.9"
+        stamped_pose="{stamped_pose}"
+        reference_frame="world"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        vector="{target_poses}"
+        input="{stamped_pose}"
+      />
+      <Action
+        ID="CreateStampedPose"
+        orientation_xyzw="0.000; 0.000; 0.000; 1.000"
+        position_xyz="0.2;0.5;0.9"
+        stamped_pose="{stamped_pose}"
+        reference_frame="world"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        vector="{target_poses}"
+        input="{stamped_pose}"
+      />
+      <Action
+        ID="CreateStampedPose"
+        orientation_xyzw="0.000; 0.000; 0.000; 1.000"
+        position_xyz="0.1;0.5;0.9"
+        stamped_pose="{stamped_pose}"
+        reference_frame="world"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        vector="{target_poses}"
+        input="{stamped_pose}"
+      />
+      <Action
+        ID="CreateStampedPose"
+        orientation_xyzw="0.000; 0.000; 0.000; 1.000"
+        position_xyz="-0.1;0.5;0.9"
+        stamped_pose="{stamped_pose}"
+        reference_frame="world"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        vector="{target_poses}"
+        input="{stamped_pose}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Create PCL Poses Vector">
+      <MetadataFields>
+        <Metadata subcategory="Vector Handling" />
+        <Metadata runnable="false" />
+      </MetadataFields>
+      <output_port name="target_poses" default="{target_poses}" />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/get_pose_at_gripper_and_display.xml b/src/lab_sim/objectives/get_pose_at_gripper_and_display.xml
new file mode 100644
index 00000000..8b2a1999
--- /dev/null
+++ b/src/lab_sim/objectives/get_pose_at_gripper_and_display.xml
@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="GetPoseAtGripperandDisplay">
+  <!--//////////-->
+  <BehaviorTree
+    ID="GetPoseAtGripperandDisplay"
+    _description="Create a pose at the gripper, transform it, and display it."
+  >
+    <Control ID="Sequence">
+      <Action
+        ID="CreateStampedPose"
+        position_xyz="0; 0; 0"
+        stamped_pose="{stamped_pose}"
+        reference_frame="pinch"
+      />
+      <Action ID="TransformPoseFrame" input_pose="{stamped_pose}" />
+      <Action ID="VisualizePose" pose="{output_pose}" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="GetPoseAtGripperandDisplay">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Pose Handling" />
+      </MetadataFields>
+      <inout_port
+        name="output_pose"
+        default="{output_pose}"
+        type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"
+      />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/mpc_pose_tracking_dynamic_point_cloud_avoidance_with_sphere_down_sample.xml b/src/lab_sim/objectives/mpc_pose_tracking_dynamic_point_cloud_avoidance_with_sphere_down_sample.xml
new file mode 100644
index 00000000..b37eb008
--- /dev/null
+++ b/src/lab_sim/objectives/mpc_pose_tracking_dynamic_point_cloud_avoidance_with_sphere_down_sample.xml
@@ -0,0 +1,121 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root
+  BTCPP_format="4"
+  main_tree_to_execute="MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample"
+>
+  <BehaviorTree
+    ID="MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample"
+    _description="Example of end effector avoiding a moving point cloud that is down-sampled with spheres."
+    _favorite="true"
+  >
+    <Control ID="Sequence">
+      <SubTree ID="Clear Snapshot" _collapsed="true" />
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        waypoint_name="Home"
+        joint_group_name="manipulator"
+        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+      />
+      <SubTree ID="Create PCL Poses Vector" _collapsed="false" />
+      <SubTree
+        ID="ComputePoseStampedFromWaypoint"
+        _collapsed="false"
+        link_pose="{link_pose}"
+      />
+      <Decorator ID="ForEach" vector_in="{target_poses}" out="{stamped_pose}">
+        <Control ID="Sequence">
+          <Action ID="VisualizePose" />
+          <Action ID="BreakpointSubscriber" _skipIf="true" />
+          <SubTree
+            ID="PlacePointCloudFromFileInUI"
+            _collapsed="false"
+            input_pose="{stamped_pose}"
+            transform_cloud="{output_cloud}"
+          />
+          <SubTree
+            ID="GetPoseAtGripperandDisplay"
+            _collapsed="false"
+            output_pose="{gripper_pose}"
+          />
+          <Action
+            ID="CreateCollisionSpheresAtClosestPoints"
+            closest_point_count="300"
+            collision_object="{collision_object}"
+            filter_cylinder_height="3.0"
+            filter_cylinder_radius="3.0"
+            point_cloud="{output_cloud}"
+            reference_pose="{gripper_pose}"
+            sphere_radius="0.1"
+            debug_output_markers="true"
+            marker_scale_xyz="{marker_scale_xyz2}"
+            marker_poses="{marker_poses2}"
+          />
+          <Action
+            ID="CreateStampedTwist"
+            reference_frame="world"
+            stamped_twist="{stamped_twist}"
+          />
+          <Action
+            ID="PublishMarkers"
+            marker_type="sphere"
+            marker_namespace="obstacles2"
+            marker_scale_xyz="{marker_scale_xyz2}"
+            marker_poses="{marker_poses2}"
+          />
+          <Control ID="Fallback">
+            <Decorator ID="Timeout" msec="3000">
+              <Control ID="Sequence">
+                <Action
+                  ID="MPCSphereClearance"
+                  differentiable="0"
+                  follow_joint_trajectory_topic="/joint_trajectory_controller/follow_joint_trajectory"
+                  gradient_num_trajectory="32"
+                  gradient_spline_points="1.000000"
+                  horizon="0.1"
+                  planning_group="manipulator"
+                  sampling_control_width="0.015000"
+                  sampling_exploration="0.050000"
+                  sampling_sample_width="0.010000"
+                  sampling_spline_points="10.000000"
+                  set_ctrl_command_to_current_state="true"
+                  timestep="0.010000"
+                  total_return="{total_return}"
+                  cartesian_acceleration="100"
+                  cartesian_acceleration_body="tool_changer"
+                  cartesian_velocity="100"
+                  cartesian_velocity_body="tool_changer"
+                  collision_sphere_object="{collision_object}"
+                  gripper_site_name="pinch"
+                  max_cartesian_acceleration_goal="500"
+                  max_cartesian_velocity_goal="0.5"
+                  mujoco_model="description/simple_scene.xml"
+                  mujoco_model_package="lab_sim"
+                  safe_distance="0.25"
+                  sphere_clearance="1.0"
+                  target_pose="{link_pose}"
+                  target_twist="{stamped_twist}"
+                  warmup_iterations="256"
+                  timeout="300"
+                  site_tracking="0.2"
+                  link_sites="attachment_site"
+                />
+              </Control>
+            </Decorator>
+            <Action ID="AlwaysSuccess" />
+          </Control>
+        </Control>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree
+      ID="MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample"
+    >
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Application - MPC Examples" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/mpc_pose_tracking_static_point_cloud_avoidance_with_sphere_down_sample.xml b/src/lab_sim/objectives/mpc_pose_tracking_static_point_cloud_avoidance_with_sphere_down_sample.xml
new file mode 100644
index 00000000..a8ace371
--- /dev/null
+++ b/src/lab_sim/objectives/mpc_pose_tracking_static_point_cloud_avoidance_with_sphere_down_sample.xml
@@ -0,0 +1,124 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root
+  BTCPP_format="4"
+  main_tree_to_execute="MPC Pose Tracking Static Point Cloud Avoidance with Sphere Down Sample"
+>
+  <BehaviorTree
+    ID="MPC Pose Tracking Static Point Cloud Avoidance with Sphere Down Sample"
+    _description="Example of end effector avoiding a point cloud that is down-sampled with spheres and statically placed in the environment."
+    _favorite="true"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree ID="Clear Snapshot" _collapsed="true" />
+      <SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Home" />
+      <Action ID="CreateStampedPose" position_xyz="0.0;0.5;0.8" />
+      <Action ID="VisualizePose" />
+      <SubTree
+        ID="PlacePointCloudFromFileInUI"
+        _collapsed="false"
+        input_pose="{stamped_pose}"
+        transform_cloud="{output_cloud}"
+      />
+      <SubTree
+        ID="ComputePoseStampedFromWaypoint"
+        _collapsed="false"
+        link_pose="{link_pose}"
+      />
+      <Action
+        ID="VisualizePose"
+        marker_lifetime="0.000000"
+        marker_name="target_pose"
+        marker_size="0.100000"
+        pose="{link_pose}"
+      />
+      <SubTree
+        ID="GetPoseAtGripperandDisplay"
+        _collapsed="false"
+        output_pose="{gripper_pose}"
+      />
+      <Action
+        ID="VisualizePose"
+        marker_lifetime="0.000000"
+        marker_name="sphere_pose"
+        marker_size="0.100000"
+        pose="{gripper_pose}"
+      />
+      <Action
+        ID="CreateCollisionSpheresAtClosestPoints"
+        closest_point_count="300"
+        collision_object="{collision_object}"
+        filter_cylinder_height="3.0"
+        filter_cylinder_radius="3.0"
+        point_cloud="{output_cloud}"
+        reference_pose="{gripper_pose}"
+        sphere_radius="0.1"
+        debug_output_markers="true"
+        marker_scale_xyz="{marker_scale_xyz2}"
+        marker_poses="{marker_poses2}"
+      />
+      <Action
+        ID="PublishMarkers"
+        marker_type="sphere"
+        marker_namespace="obstacles2"
+        marker_scale_xyz="{marker_scale_xyz2}"
+        marker_poses="{marker_poses2}"
+      />
+      <Action
+        ID="CreateStampedTwist"
+        reference_frame="world"
+        stamped_twist="{stamped_twist}"
+      />
+      <Control ID="Fallback">
+        <Decorator ID="Timeout" msec="40000">
+          <Control ID="Sequence">
+            <Action
+              ID="MPCSphereClearance"
+              differentiable="0"
+              follow_joint_trajectory_topic="/joint_trajectory_controller/follow_joint_trajectory"
+              gradient_num_trajectory="32"
+              gradient_spline_points="1.000000"
+              horizon="0.1"
+              planning_group="manipulator"
+              sampling_control_width="0.015000"
+              sampling_exploration="0.050000"
+              sampling_sample_width="0.010000"
+              sampling_spline_points="10.000000"
+              set_ctrl_command_to_current_state="true"
+              timestep="0.010000"
+              total_return="{total_return}"
+              cartesian_acceleration="100"
+              cartesian_acceleration_body="tool_changer"
+              cartesian_velocity="100"
+              cartesian_velocity_body="tool_changer"
+              collision_sphere_object="{collision_object}"
+              gripper_site_name="pinch"
+              max_cartesian_acceleration_goal="500"
+              max_cartesian_velocity_goal="0.5"
+              mujoco_model="description/simple_scene.xml"
+              mujoco_model_package="lab_sim"
+              safe_distance="0.25"
+              sphere_clearance="1.0"
+              target_pose="{link_pose}"
+              target_twist="{stamped_twist}"
+              warmup_iterations="256"
+              timeout="300"
+              site_tracking="0.2"
+              link_sites="attachment_site"
+            />
+          </Control>
+        </Decorator>
+        <Action ID="AlwaysSuccess" />
+      </Control>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree
+      ID="MPC Pose Tracking Static Point Cloud Avoidance with Sphere Down Sample"
+    >
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Application - MPC Examples" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/mpc_pose_tracking_with_point_cloud_avoidance.xml b/src/lab_sim/objectives/mpc_pose_tracking_with_point_cloud_avoidance.xml
index 6666798a..0a0b4e41 100644
--- a/src/lab_sim/objectives/mpc_pose_tracking_with_point_cloud_avoidance.xml
+++ b/src/lab_sim/objectives/mpc_pose_tracking_with_point_cloud_avoidance.xml
@@ -7,7 +7,7 @@
   <BehaviorTree
     ID="MPC Pose Tracking With Point Cloud Avoidance"
     _description="Uses Model Predictive Control (MPC) to track a target pose while avoiding obstacles represented by a point cloud."
-    _favorite="false"
+    _favorite="true"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <SubTree ID="Clear Snapshot" _collapsed="true" />
@@ -67,7 +67,7 @@
         ID="CreateStampedPose"
         reference_frame="pinch"
         stamped_pose="{stamped_pose}"
-        position_xyz="0;0;0.1"
+        position_xyz="0;0.3;0.1"
       />
       <Action
         ID="TransformPoseFrame"
@@ -88,7 +88,7 @@
         stamped_twist="{stamped_twist}"
       />
       <Control ID="Fallback">
-        <Decorator ID="Timeout" msec="5000">
+        <Decorator ID="Timeout" msec="7000">
           <Control ID="Sequence">
             <Action
               ID="MPCPointCloudClearance"
@@ -118,11 +118,11 @@
               mujoco_model="description/simple_scene.xml"
               mujoco_model_package="lab_sim"
               point_cloud="{point_cloud}"
-              point_cloud_clearance="1.0"
+              point_cloud_clearance="1.1"
               safe_distance="0.25"
-              site_tracking="1.0"
               target_pose="{stamped_pose}"
               target_twist="{stamped_twist}"
+              site_tracking="0.45"
             />
           </Control>
         </Decorator>
diff --git a/src/lab_sim/objectives/place_point_cloud_from_file_in_ui.xml b/src/lab_sim/objectives/place_point_cloud_from_file_in_ui.xml
new file mode 100644
index 00000000..e9103226
--- /dev/null
+++ b/src/lab_sim/objectives/place_point_cloud_from_file_in_ui.xml
@@ -0,0 +1,51 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="PlacePointCloudFromFileInUI">
+  <!--//////////-->
+  <BehaviorTree
+    ID="PlacePointCloudFromFileInUI"
+    _description="Load point cloud from file, transform it using the input pose, and send it to the UI."
+  >
+    <Control ID="Sequence" _collapsed="false">
+      <Action
+        ID="LoadPointCloudFromFile"
+        package_name="lab_sim"
+        file_path="description/assets/cube_0_5.stl"
+        color="0;0;255"
+      />
+      <Action
+        ID="TransformPointCloudFrame"
+        input_cloud="{point_cloud}"
+        output_cloud="{world_cloud}"
+      />
+      <Action
+        ID="TransformPointCloud"
+        transform_pose="{input_pose}"
+        output_cloud="{transform_cloud}"
+        input_cloud="{world_cloud}"
+      />
+      <Action
+        ID="SendPointCloudToUI"
+        pcd_topic="/pcd_pointcloud_captures"
+        point_cloud="{transform_cloud}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="PlacePointCloudFromFileInUI">
+      <MetadataFields>
+        <Metadata runnable="false" />
+        <Metadata subcategory="Perception - 3D Point Cloud" />
+      </MetadataFields>
+      <inout_port
+        name="input_pose"
+        default="{stamped_pose}"
+        type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"
+      />
+      <inout_port
+        name="transform_cloud"
+        default="{output_cloud}"
+        type="sensor_msgs::msg::PointCloud2_&lt;std::allocator&lt;void&gt; &gt;"
+      />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/waypoints/ur_waypoints.yaml b/src/lab_sim/waypoints/ur_waypoints.yaml
index b7c32109..a74e2aa0 100644
--- a/src/lab_sim/waypoints/ur_waypoints.yaml
+++ b/src/lab_sim/waypoints/ur_waypoints.yaml
@@ -1288,3 +1288,89 @@
     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