Add Objectives for showing mpc pushing end-effector with spheres down sampled from a pcl

[bkanator]

Added two objectives, and some subtree objectives

Added a static objective

static_mpc_spheres-2026-01-08_17.31.29.mp4

Also added a 'dynamic' (I tried but open to better ways) objective

dynamic_mpc_spheres2-2026-01-08_17.33.16.mp4

[Copilot]

Pull request overview

This pull request adds new MPC (Model Predictive Control) objectives that demonstrate pushing an end-effector while avoiding obstacles represented as point clouds down-sampled with spheres. The PR includes both a static and a dynamic version of the objective, along with supporting subtree utilities for point cloud manipulation and pose computation.

Key changes:

• Addition of two new robot waypoints (behind_flask and near_rack) for the UR robot
• New static objective for MPC with sphere-based point cloud avoidance
• New dynamic objective that iterates through multiple poses while avoiding moving point clouds
• Supporting subtrees for loading point clouds from files, computing poses from waypoints, and displaying gripper poses

Reviewed changes

Copilot reviewed 8 out of 8 changed files in this pull request and generated 9 comments.

Show a summary per file

File	Description
ur_waypoints.yaml	Adds two new waypoints (behind_flask and near_rack) with complete joint state configurations
placepointcloudfromfileinui.xml	New subtree that loads a point cloud from an STL file, transforms it, and sends it to the UI for visualization
mpc_pose_tracking_with_point_cloud_avoidance.xml	Reformatted and updated parameters (timeout, position offset, point cloud clearance, site tracking)
mpc_pose_tracking_static_point_cloud_avoidance_with_sphere_down_sample.xml	New objective demonstrating static point cloud avoidance using sphere down-sampling with MPC
mpc_pose_tracking_dynamic_point_cloud_avoidance_with_sphere_down_sample.xml	New objective demonstrating dynamic point cloud avoidance by iterating through multiple poses
getposeatgripperanddisplay.xml	New subtree that retrieves and visualizes the current gripper pose
create_pcl_poses_vector.xml	New subtree that creates a vector of stamped poses for the dynamic objective
computeposestampedfromwaypoint.xml	New subtree that computes a link pose from a waypoint using forward kinematics

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[bkanator]

FYI I don't think CI will pass for these new objectives until the PR for moveit_pro is in because it needs the new visualization ports (so need to re-do CI once that is merged) https://github.com/PickNikRobotics/moveit_pro/pull/16107

Error: jective_server_node_main-16] [2026-01-13 20:41:58.205] [objective_server_node] [error] Objective Server Error: Failed to create behavior tree for Objective MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample. Reason: BehaviorTree error while creating tree: a port with name [debug_output_markers] is found in the XML (, line 15) but not in the providedPorts() of its registered node type.