From 6f538f3503f0849863f90de819d2fb882e6a86fb Mon Sep 17 00:00:00 2001
From: shaur-k <shaurkinthewater@gmail.com>
Date: Thu, 3 Apr 2025 12:08:03 -0600
Subject: [PATCH] fix: xml formatting matches the pretty-print lxml setting

---
 .pre-commit-config.yaml                       |    9 +-
 .../config/tree_nodes_model.xml               |    2 +-
 .../example_behaviors_plugin_description.xml  |    7 +-
 src/example_behaviors/package.xml             |    2 +-
 src/factory_sim/description/box-welds.xml     |  577 +--
 src/factory_sim/description/scene.xml         |  322 +-
 src/factory_sim/description/tool.xml          |   39 +-
 .../launch/agent_bridge.launch.xml            |    6 +-
 .../objectives/activate_vacuum.xml            |   11 +-
 src/factory_sim/objectives/clear_snapshot.xml |   11 +-
 .../objectives/deactivate_vacuum.xml          |   15 +-
 .../objectives/interpolate_to_joint_state.xml |   42 +-
 .../objectives/move_to_joint_state.xml        |   54 +-
 src/factory_sim/objectives/move_to_pose.xml   |   44 +-
 .../objectives/move_to_waypoint.xml           |   65 +-
 src/factory_sim/objectives/pick_up_tool.xml   |  101 +-
 .../objectives/place_tool_in_tool_holder.xml  |   92 +-
 .../reachability_analysis_-_blocks.xml        |   37 +-
 .../reachability_analysis_-_bowls.xml         |   25 +-
 .../reachability_analysis_for_mesh.xml        |  202 +-
 .../reachability_analysis_for_plane.xml       |  327 +-
 .../objectives/request_teleoperation.xml      |  138 +-
 .../objectives/reset_planning_scene.xml       |   13 +-
 src/factory_sim/objectives/retract.xml        |   52 +-
 .../setup_starting_planning_scene.xml         |   23 +-
 src/factory_sim/objectives/take_snapshot.xml  |   25 +-
 src/factory_sim/objectives/teleoperate.xml    |   19 +-
 .../objectives/tool_attachment_example.xml    |   20 +-
 ..._trajectory_approval_if_user_available.xml |   21 +-
 src/factory_sim/package.xml                   |    2 +-
 src/grinding_sim/description/scene.xml        |  446 +-
 src/grinding_sim/description/tool0.xml        |  125 +-
 src/grinding_sim/description/tool90.xml       |  141 +-
 .../launch/agent_bridge.launch.xml            |    6 +-
 .../objectives/capture_point_cloud.xml        |   48 +-
 .../objectives/grind_machined_part.xml        |  135 +-
 .../load_engine_block_pointcloud.xml          |   40 +-
 .../objectives/move_to_loaded_pose.xml        |   30 +-
 src/grinding_sim/objectives/move_to_pose.xml  |   37 +-
 .../objectives/move_to_pose_imarker.xml       |   58 +-
 .../objectives/register_machined_part.xml     |  119 +-
 .../objectives/request_teleoperation.xml      |  126 +-
 .../objectives/save_tool_pose_yaml.xml        |   32 +-
 src/grinding_sim/objectives/take_snapshot.xml |   27 +-
 .../objectives/visualize_poses.xml            |   70 +-
 ..._trajectory_approval_if_user_available.xml |   19 +-
 src/grinding_sim/package.xml                  |    2 +-
 .../description/front_left_wheel_link.xml     |  382 +-
 .../description/front_right_wheel_link.xml    |  382 +-
 src/hangar_sim/description/hangar.xml         | 4411 ++++-------------
 src/hangar_sim/description/hangar_scene.xml   |  104 +-
 .../description/rear_left_wheel_link.xml      |  382 +-
 .../description/rear_right_wheel_link.xml     |  382 +-
 src/hangar_sim/description/ur5e_ridgeback.xml |  626 +--
 src/hangar_sim/description/vacuum.xml         |   78 +-
 src/hangar_sim/launch/agent_bridge.launch.xml |    6 +-
 ...oveit_mecanum_drive_controller_plugins.xml |    8 +-
 .../cartesian_draw_geometry_from_file.xml     |   79 +-
 ...cartesian_path_with_collision_checking.xml |   93 +-
 .../cartesian_plan_simple_square.xml          |  141 +-
 src/hangar_sim/objectives/close_gripper.xml   |   15 +-
 .../objectives/find_and_spray_plane.xml       |   93 +-
 .../generate_grid_pattern_on_airplane.xml     |   84 +-
 .../get_point_cloud_center_pose.xml           |   52 +-
 .../objectives/interpolate_to_joint_state.xml |   45 +-
 .../objectives/mobile_scan_and_plan.xml       |   86 +-
 .../objectives/move_boxes_to_loading_zone.xml |   50 +-
 ...ve_boxes_to_loading_zone_from_waypoint.xml |  341 +-
 src/hangar_sim/objectives/move_forward_2m.xml |   36 +-
 .../objectives/move_to_a_stampedpose.xml      |   63 +-
 .../objectives/move_to_arm_upright.xml        |   28 +-
 .../objectives/move_to_look_at_plane.xml      |   28 +-
 src/hangar_sim/objectives/move_to_pose.xml    |   60 +-
 .../objectives/move_to_pose_no_preview.xml    |   57 +-
 .../objectives/navigate_to_clicked_point.xml  |   57 +-
 src/hangar_sim/objectives/open_gripper.xml    |   15 +-
 .../objectives/request_teleoperation.xml      |  140 +-
 .../objectives/segment_image_from_point.xml   |   24 +-
 .../segment_image_from_text_prompt.xml        |   27 +-
 ...segment_point_cloud_from_clicked_point.xml |   29 +-
 src/hangar_sim/objectives/take_snap.xml       |   15 +-
 src/hangar_sim/objectives/wrist_snap.xml      |   13 +-
 src/hangar_sim/package.xml                    |    2 +-
 src/lab_sim/description/scene.xml             |   79 +-
 src/lab_sim/launch/agent_bridge.launch.xml    |    6 +-
 .../objectives/3_waypoint_pick_and_place.xml  |  103 +-
 src/lab_sim/objectives/_scan_scene.xml        |   34 +-
 .../objectives/add_waypoints_to_mtc_task.xml  |   48 +-
 .../objectives/apriltag_pick_object.xml       |  125 +-
 .../objectives/clear_previous_obstacles.xml   |   13 +-
 src/lab_sim/objectives/close_gripper.xml      |   22 +-
 .../constrained_pick_and_place_subtree.xml    |  123 +-
 .../objectives/constrained_pick_place.xml     |   55 +-
 src/lab_sim/objectives/create_pose_vector.xml |  128 +-
 .../objectives/cycle_between_waypoints.xml    |   23 +-
 src/lab_sim/objectives/force_relaxation.xml   |   48 +-
 .../get_april_tag_pose_from_image.xml         |   57 +-
 .../objectives/get_transform_frame_pose.xml   |   35 +-
 .../grasp_object_from_text_prompt.xml         |  135 +-
 src/lab_sim/objectives/grasp_planning.xml     |  116 +-
 .../grasp_pose_tuning_with_april_tag.xml      |  117 +-
 .../objectives/grasp_pose_using_yaml.xml      |  141 +-
 src/lab_sim/objectives/joint_diagnostic.xml   |   33 +-
 .../load_and_execute_joint_trajectory.xml     |   57 +-
 .../load_mesh_as_green_poincloud.xml          |   36 +-
 .../load_mesh_as_red_pointcloud.xml           |   36 +-
 src/lab_sim/objectives/look_at_table.xml      |   19 +-
 .../objectives/loop_detect_apriltag.xml       |   40 +-
 src/lab_sim/objectives/move_along_square.xml  |   96 +-
 .../move_with_velocity_and_force.xml          |   92 +-
 src/lab_sim/objectives/open_gripper.xml       |   16 +-
 .../objectives/pick_and_place_example.xml     |   91 +-
 .../pick_april_tag_object_with_approval.xml   |  135 +-
 src/lab_sim/objectives/pick_from_pose.xml     |   47 +-
 src/lab_sim/objectives/pick_object.xml        |   37 +-
 src/lab_sim/objectives/pick_up_cube.xml       |   18 +-
 src/lab_sim/objectives/place_at_pose.xml      |   57 +-
 src/lab_sim/objectives/place_object.xml       |   29 +-
 .../objectives/plan_and_save_trajectory.xml   |   58 +-
 src/lab_sim/objectives/plan_move_to_pose.xml  |   40 +-
 src/lab_sim/objectives/plan_to_pose.xml       |   56 +-
 .../objectives/playback_square_trajectory.xml |   49 +-
 .../push_button_with_a_trajectory.xml         |   96 +-
 .../record_and_replay_scanning_motion.xml     |   17 +-
 .../objectives/record_square_trajectory.xml   |   57 +-
 src/lab_sim/objectives/register_cad_part.xml  |   71 +-
 .../objectives/request_teleoperation.xml      |  135 +-
 src/lab_sim/objectives/sample_april_tag.xml   |   64 +-
 .../segment_image_from_text_prompt.xml        |   32 +-
 ...segment_point_cloud_from_clicked_point.xml |   29 +-
 .../segment_point_cloud_from_text_prompt.xml  |   36 +-
 .../take_picture_with_wrist_camera.xml        |   41 +-
 src/lab_sim/objectives/take_snap.xml          |   16 +-
 src/lab_sim/objectives/wrist_snap.xml         |   14 +-
 src/lab_sim/package.xml                       |    2 +-
 .../launch/agent_bridge.launch.xml            |    6 +-
 .../objectives/clear_snapshot.xml             |   11 +-
 .../objectives/close_gripper.xml              |   10 +-
 ...t_imarker_pose_from_mesh_visualization.xml |   34 +-
 .../objectives/move_along_path.xml            |  110 +-
 .../objectives/move_to_joint_state.xml        |   52 +-
 .../objectives/open_gripper.xml               |   10 +-
 .../objectives/reactivate_gripper.xml         |    9 +-
 .../kinova_gen3_base_config/package.xml       |    2 +-
 .../description/mujoco/gen3_7dof.xml          |  795 +--
 .../mujoco/gen3_7dof_rafti_fingers.xml        |  905 +---
 .../description/mujoco/landsat_scene.xml      |  154 +-
 .../mujoco/rafti_fingers_scene.xml            |  249 +-
 .../kinova_sim/description/mujoco/scene.xml   |  249 +-
 .../kinova_sim/launch/agent_bridge.launch.xml |    5 +-
 .../april_tag_object_registration.xml         |  263 +-
 .../kinova_sim/objectives/close_gripper.xml   |   12 +-
 .../objectives/compliant_grasp_rafti.xml      |  260 +-
 .../objectives/compliant_grasp_rafti_vfc.xml  |  276 +-
 .../objectives/demo_trajectory_stitching.xml  |  148 +-
 .../kinova_sim/objectives/move_along_path.xml |  127 +-
 .../objectives/move_along_path_admittance.xml |  121 +-
 .../kinova_sim/objectives/move_to_pose.xml    |   74 +-
 .../kinova_sim/objectives/open_gripper.xml    |   10 +-
 .../re_zero_force_torque_sensors.xml          |   16 +-
 .../objectives/reactivate_gripper.xml         |    9 +-
 .../objectives/sample_april_tag.xml           |   56 +-
 .../kinova_sim/objectives/take_snapshot.xml   |   16 +-
 .../velocity_force_controller_zero.xml        |   36 +-
 .../visual_servo_teach_reference.xml          |   57 +-
 .../objectives/visual_servo_to_reference.xml  |   95 +-
 .../kinova_sim/objectives/write_picknik.xml   |   65 +-
 .../kinova_sim/package.xml                    |    2 +-
 .../package.xml                               |    2 +-
 .../description/mujoco/gen3_7dof_assets.xml   |  317 +-
 .../description/mujoco/gen3_7dof_body.xml     |  592 +--
 .../description/mujoco/scene.xml              |  387 +-
 .../launch/agent_bridge.launch.xml            |    5 +-
 .../objectives/close_gripper.xml              |   10 +-
 .../objectives/compliant_grasp_landsat.xml    |  142 +-
 .../objectives/open_gripper.xml               |   10 +-
 .../re_zero_force_torque_sensors.xml          |   16 +-
 .../objectives/sample_april_tag.xml           |   56 +-
 .../objectives/take_snapshot.xml              |   16 +-
 .../space_satellite_sim/package.xml           |    2 +-
 .../description/scene.xml                     |   57 +-
 .../launch/agent_bridge.launch.xml            |    6 +-
 .../00_solution_-_move_to_initial_pose.xml    |   24 +-
 .../01_solution_-_create_a_pose.xml           |   18 +-
 .../02_solution_-_visualize_a_pose.xml        |   20 +-
 .../03_solution_-_pose_2m_above_the_robot.xml |   25 +-
 .../04_solution_-_move_to_a_pose.xml          |   28 +-
 .../05_solution_-_define_a_path.xml           |   29 +-
 .../06_solution_-_visualize_a_path.xml        |   31 +-
 .../07_solution_-_plan_motion_for_a_path.xml  |   29 +-
 .../08_solution_-_execute_path_plan.xml       |   26 +-
 .../09_solution_-_an_infeasible_path.xml      |   28 +-
 .../10_solution_-_visualize_path_error.xml    |   38 +-
 .../11_solution_-_draw_a_square.xml           |   58 +-
 .../12_solution_-_constrain_orientation.xml   |   63 +-
 .../13_solution_-_load_a_path_from_file.xml   |   33 +-
 .../14_solution_-_load_from_file_execute.xml  |   48 +-
 .../15_solution_-_a_path_that_collides.xml    |   36 +-
 .../16_solution_-_checking_for_collisions.xml |   46 +-
 ...7_solution_-_execute_the_feasible_path.xml |   51 +-
 .../18_solution_-_define_a_coverage_path.xml  |   36 +-
 .../19_solution_-_execute_coverage_path.xml   |   46 +-
 .../20_solution_-_look_at_the_airfoil.xml     |   19 +-
 .../21_solution_-_find_the_airfoil.xml        |   31 +-
 .../22_solution_-_visualize_airfoil_frame.xml |   38 +-
 .../23_solution_-_airfoil_coverage_origin.xml |   45 +-
 .../24_solution_-_airfoil_coverage_path.xml   |   55 +-
 ...25_solution_-_execute_airfoil_coverage.xml |   66 +-
 .../26_solution_-_better_coverage.xml         |   72 +-
 .../objectives/draw_picknik.xml               |   80 +-
 .../objectives/draw_square.xml                |  130 +-
 .../objectives/estimate_object_pose.xml       |   80 +-
 .../objectives/follow_path.xml                |  100 +-
 .../objectives/generate_coverage_path.xml     |   63 +-
 .../objectives/move_2m_pose.xml               |   50 +-
 .../objectives/move_to_a_stampedpose.xml      |   63 +-
 .../objectives/scan_and_plan.xml              |   95 +-
 .../objectives/take_snapshot.xml              |   30 +-
 .../mobile_manipulation_config/package.xml    |    2 +-
 .../description/scene.xml                     |   40 +-
 .../description/ur5e.xml                      |  995 +---
 .../launch/agent_bridge.launch.xml            |    6 +-
 .../objectives/apriltag_pick_object.xml       |   58 +-
 .../pick_first_object_in_vector.xml           |   45 +-
 .../objectives/pick_object.xml                |   57 +-
 .../objectives/pick_up_cube.xml               |    8 +-
 .../objectives/take_snap.xml                  |   16 +-
 .../objectives/wrist_snap.xml                 |   14 +-
 .../picknik_ur_mobile_config/package.xml      |    2 +-
 .../mock_sim/launch/agent_bridge.launch.xml   |    6 +-
 .../objectives/3_waypoint_pick_and_place.xml  |  105 +-
 .../objectives/constrained_pick_place.xml     |   33 +-
 .../objectives/cycle_between_waypoints.xml    |   24 +-
 .../mock_sim/objectives/joint_diagnostic.xml  |   54 +-
 .../mock_sim/objectives/move_along_path.xml   |  124 +-
 .../mock_sim/objectives/rrt_connect_test.xml  |   38 +-
 .../mock_sim/package.xml                      |    2 +-
 .../package.xml                               |    2 +-
 .../launch/agent_bridge.launch.xml            |    6 +-
 .../objectives/close_left_gripper.xml         |   15 +-
 .../objectives/close_right_gripper.xml        |   10 +-
 .../objectives/interpolate_to_joint_state.xml |   39 +-
 .../objectives/move_to_joint_state.xml        |   52 +-
 .../multi_arm_sim/objectives/move_to_pose.xml |   41 +-
 .../objectives/move_to_waypoint.xml           |   55 +-
 .../objectives/move_two_arms.xml              |   41 +-
 .../objectives/multi_tip_ik_example.xml       |   95 +-
 .../objectives/open_left_gripper.xml          |   10 +-
 .../objectives/open_right_gripper.xml         |   10 +-
 .../objectives/request_teleoperation.xml      |  119 +-
 ..._trajectory_approval_if_user_available.xml |   18 +-
 .../multi_arm_sim/package.xml                 |    2 +-
 .../launch/agent_bridge.launch.xml            |    6 +-
 .../launch/moveit_pro.launch.xml              |   13 +-
 .../launch/rs_cameras.launch.xml              |   92 +-
 .../launch/ur_tool_comms.launch.xml           |   20 +-
 .../objectives/close_gripper.xml              |   20 +-
 .../create_point_cloud_vector_from_masks.xml  |   48 +-
 .../objectives/open_gripper.xml               |   16 +-
 ...e_from_no_negative_text_prompt_subtree.xml |   69 +-
 .../segment_image_from_point_subtree.xml      |   71 +-
 ...segment_image_from_text_prompt_subtree.xml |   68 +-
 ...point_cloud_from_clicked_point_subtree.xml |   86 +-
 ...t_point_cloud_from_text_prompt_subtree.xml |  131 +-
 .../visualize_segmented_point_cloud.xml       |   45 +-
 .../picknik_ur_base_config/package.xml        |    2 +-
 ...pstop_reset_service_plugin_description.xml |    7 +-
 .../launch/agent_bridge.launch.xml            |    6 +-
 .../objectives/close_cabinet_door.xml         |  135 +-
 .../detect_door_graspable_object.xml          |   86 +-
 .../detect_fixed_handle_graspable_object.xml  |  109 +-
 .../objectives/detect_hinge_line_segment.xml  |   71 +-
 .../detect_lever_handle_line_segment.xml      |   82 +-
 .../objectives/find_cuboids.xml               |   29 +-
 .../objectives/force_exceeds_threshold.xml    |   19 +-
 .../objectives/inspect_surface.xml            |   71 +-
 .../objectives/loop_detect_apriltag.xml       |   36 +-
 .../looping_pick_and_place_object.xml         |  176 +-
 .../objectives/open_cabinet_door.xml          |  177 +-
 .../objectives/open_door_fixed_handle_ml.xml  |  168 +-
 .../objectives/open_door_lever_handle_ml.xml  |  118 +-
 .../objectives/open_lever_handle_door.xml     |  153 +-
 .../pick_apriltag_labeled_object.xml          |   52 +-
 .../objectives/pick_object.xml                |   79 +-
 .../objectives/pick_place_object.xml          |  282 +-
 .../objectives/push_button.xml                |  134 +-
 .../objectives/push_button_ml.xml             |  191 +-
 .../re_zero_force_torque_sensors.xml          |   16 +-
 .../objectives/reactivate_gripper.xml         |   11 +-
 .../objectives/retreat_to_initial_pose.xml    |   69 +-
 .../objectives/sam_pick_object.xml            |  159 +-
 .../objectives/sample_april_tag.xml           |   56 +-
 .../objectives/take_snapshot.xml              |   31 +-
 .../teach_apriltag_grasp_offset.xml           |   77 +-
 .../update_admittance_controller.xml          |   16 +-
 .../visual_servo_teach_reference.xml          |   59 +-
 .../objectives/visual_servo_to_reference.xml  |   95 +-
 .../picknik_ur_site_config/package.xml        |    2 +-
 298 files changed, 5852 insertions(+), 22601 deletions(-)

diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index a5f38f86..3feb0a0f 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -78,13 +78,10 @@ repos:
         types: ["dockerfile"]
         entry: hadolint/hadolint:2.11.0 hadolint
 
-  - repo: https://github.com/pre-commit/mirrors-prettier
-    rev: "v3.1.0"
+  - repo: https://github.com/GlodoUK/pre-commit-xmllint
+    rev: b9281d1cf34246ed43f2dd2634b855d04d06cb80
     hooks:
       # Use Prettier to lint XML files because, well.. its rules are prettier than most linters, as the name implies.
       # Also we use it in the UI, so it's familiar.
-      - id: prettier
-        additional_dependencies:
-          - "prettier@3.1.0"
-          - "@prettier/plugin-xml@3.3.1"
+      - id: lxml-format
         files: \.(xml)$
diff --git a/src/example_behaviors/config/tree_nodes_model.xml b/src/example_behaviors/config/tree_nodes_model.xml
index f3844c22..2f5356cb 100644
--- a/src/example_behaviors/config/tree_nodes_model.xml
+++ b/src/example_behaviors/config/tree_nodes_model.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root>
   <TreeNodesModel>
     <!-- Include additional SubTree metadata in this file. -->
diff --git a/src/example_behaviors/example_behaviors_plugin_description.xml b/src/example_behaviors/example_behaviors_plugin_description.xml
index 347594f9..1268a766 100644
--- a/src/example_behaviors/example_behaviors_plugin_description.xml
+++ b/src/example_behaviors/example_behaviors_plugin_description.xml
@@ -1,7 +1,4 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <library path="example_behaviors">
-  <class
-    type="example_behaviors::ExampleBehaviorsLoader"
-    base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase"
-  />
+  <class type="example_behaviors::ExampleBehaviorsLoader" base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase"/>
 </library>
diff --git a/src/example_behaviors/package.xml b/src/example_behaviors/package.xml
index 0ac3ce5d..9b754002 100644
--- a/src/example_behaviors/package.xml
+++ b/src/example_behaviors/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>example_behaviors</name>
   <version>6.5.0</version>
diff --git a/src/factory_sim/description/box-welds.xml b/src/factory_sim/description/box-welds.xml
index 4c3df10c..e750d511 100644
--- a/src/factory_sim/description/box-welds.xml
+++ b/src/factory_sim/description/box-welds.xml
@@ -1,508 +1,77 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="fanuc_cube_welds">
   <equality>
-    <weld
-      name="suction_cup_sphere-0-0-0-0"
-      body1="sphere-0-0-0-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-0-1"
-      body1="sphere-0-0-0-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-0-2"
-      body1="sphere-0-0-0-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-1-0"
-      body1="sphere-0-0-1-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-1-1"
-      body1="sphere-0-0-1-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-1-2"
-      body1="sphere-0-0-1-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-2-0"
-      body1="sphere-0-0-2-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-2-1"
-      body1="sphere-0-0-2-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-2-2"
-      body1="sphere-0-0-2-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-3-0"
-      body1="sphere-0-0-3-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-3-1"
-      body1="sphere-0-0-3-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-3-2"
-      body1="sphere-0-0-3-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-4-0"
-      body1="sphere-0-0-4-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-4-1"
-      body1="sphere-0-0-4-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-4-2"
-      body1="sphere-0-0-4-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-5-0"
-      body1="sphere-0-0-5-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-5-1"
-      body1="sphere-0-0-5-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-0-5-2"
-      body1="sphere-0-0-5-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-0-0"
-      body1="sphere-0-1-0-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-0-1"
-      body1="sphere-0-1-0-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-0-2"
-      body1="sphere-0-1-0-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-1-0"
-      body1="sphere-0-1-1-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-1-1"
-      body1="sphere-0-1-1-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-1-2"
-      body1="sphere-0-1-1-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-2-0"
-      body1="sphere-0-1-2-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-2-1"
-      body1="sphere-0-1-2-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-2-2"
-      body1="sphere-0-1-2-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-3-0"
-      body1="sphere-0-1-3-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-3-1"
-      body1="sphere-0-1-3-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-3-2"
-      body1="sphere-0-1-3-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-4-0"
-      body1="sphere-0-1-4-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-4-1"
-      body1="sphere-0-1-4-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-4-2"
-      body1="sphere-0-1-4-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-5-0"
-      body1="sphere-0-1-5-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-5-1"
-      body1="sphere-0-1-5-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-0-1-5-2"
-      body1="sphere-0-1-5-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-0-0"
-      body1="sphere-1-0-0-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-0-1"
-      body1="sphere-1-0-0-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-0-2"
-      body1="sphere-1-0-0-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-1-0"
-      body1="sphere-1-0-1-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-1-1"
-      body1="sphere-1-0-1-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-1-2"
-      body1="sphere-1-0-1-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-2-0"
-      body1="sphere-1-0-2-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-2-1"
-      body1="sphere-1-0-2-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-2-2"
-      body1="sphere-1-0-2-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-3-0"
-      body1="sphere-1-0-3-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-3-1"
-      body1="sphere-1-0-3-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-3-2"
-      body1="sphere-1-0-3-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-4-0"
-      body1="sphere-1-0-4-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-4-1"
-      body1="sphere-1-0-4-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-4-2"
-      body1="sphere-1-0-4-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-5-0"
-      body1="sphere-1-0-5-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-5-1"
-      body1="sphere-1-0-5-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-0-5-2"
-      body1="sphere-1-0-5-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-0-0"
-      body1="sphere-1-1-0-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-0-1"
-      body1="sphere-1-1-0-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-0-2"
-      body1="sphere-1-1-0-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-1-0"
-      body1="sphere-1-1-1-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-1-1"
-      body1="sphere-1-1-1-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-1-2"
-      body1="sphere-1-1-1-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-2-0"
-      body1="sphere-1-1-2-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-2-1"
-      body1="sphere-1-1-2-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-2-2"
-      body1="sphere-1-1-2-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-3-0"
-      body1="sphere-1-1-3-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-3-1"
-      body1="sphere-1-1-3-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-3-2"
-      body1="sphere-1-1-3-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-4-0"
-      body1="sphere-1-1-4-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-4-1"
-      body1="sphere-1-1-4-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-4-2"
-      body1="sphere-1-1-4-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-5-0"
-      body1="sphere-1-1-5-0"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-5-1"
-      body1="sphere-1-1-5-1"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
-    <weld
-      name="suction_cup_sphere-1-1-5-2"
-      body1="sphere-1-1-5-2"
-      body2="tool_tip"
-      active="false"
-      torquescale="1"
-    />
+    <weld name="suction_cup_sphere-0-0-0-0" body1="sphere-0-0-0-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-0-1" body1="sphere-0-0-0-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-0-2" body1="sphere-0-0-0-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-1-0" body1="sphere-0-0-1-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-1-1" body1="sphere-0-0-1-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-1-2" body1="sphere-0-0-1-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-2-0" body1="sphere-0-0-2-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-2-1" body1="sphere-0-0-2-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-2-2" body1="sphere-0-0-2-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-3-0" body1="sphere-0-0-3-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-3-1" body1="sphere-0-0-3-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-3-2" body1="sphere-0-0-3-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-4-0" body1="sphere-0-0-4-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-4-1" body1="sphere-0-0-4-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-4-2" body1="sphere-0-0-4-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-5-0" body1="sphere-0-0-5-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-5-1" body1="sphere-0-0-5-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-0-5-2" body1="sphere-0-0-5-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-0-0" body1="sphere-0-1-0-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-0-1" body1="sphere-0-1-0-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-0-2" body1="sphere-0-1-0-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-1-0" body1="sphere-0-1-1-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-1-1" body1="sphere-0-1-1-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-1-2" body1="sphere-0-1-1-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-2-0" body1="sphere-0-1-2-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-2-1" body1="sphere-0-1-2-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-2-2" body1="sphere-0-1-2-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-3-0" body1="sphere-0-1-3-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-3-1" body1="sphere-0-1-3-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-3-2" body1="sphere-0-1-3-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-4-0" body1="sphere-0-1-4-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-4-1" body1="sphere-0-1-4-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-4-2" body1="sphere-0-1-4-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-5-0" body1="sphere-0-1-5-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-5-1" body1="sphere-0-1-5-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-0-1-5-2" body1="sphere-0-1-5-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-0-0" body1="sphere-1-0-0-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-0-1" body1="sphere-1-0-0-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-0-2" body1="sphere-1-0-0-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-1-0" body1="sphere-1-0-1-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-1-1" body1="sphere-1-0-1-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-1-2" body1="sphere-1-0-1-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-2-0" body1="sphere-1-0-2-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-2-1" body1="sphere-1-0-2-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-2-2" body1="sphere-1-0-2-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-3-0" body1="sphere-1-0-3-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-3-1" body1="sphere-1-0-3-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-3-2" body1="sphere-1-0-3-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-4-0" body1="sphere-1-0-4-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-4-1" body1="sphere-1-0-4-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-4-2" body1="sphere-1-0-4-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-5-0" body1="sphere-1-0-5-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-5-1" body1="sphere-1-0-5-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-0-5-2" body1="sphere-1-0-5-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-0-0" body1="sphere-1-1-0-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-0-1" body1="sphere-1-1-0-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-0-2" body1="sphere-1-1-0-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-1-0" body1="sphere-1-1-1-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-1-1" body1="sphere-1-1-1-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-1-2" body1="sphere-1-1-1-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-2-0" body1="sphere-1-1-2-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-2-1" body1="sphere-1-1-2-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-2-2" body1="sphere-1-1-2-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-3-0" body1="sphere-1-1-3-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-3-1" body1="sphere-1-1-3-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-3-2" body1="sphere-1-1-3-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-4-0" body1="sphere-1-1-4-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-4-1" body1="sphere-1-1-4-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-4-2" body1="sphere-1-1-4-2" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-5-0" body1="sphere-1-1-5-0" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-5-1" body1="sphere-1-1-5-1" body2="tool_tip" active="false" torquescale="1"/>
+    <weld name="suction_cup_sphere-1-1-5-2" body1="sphere-1-1-5-2" body2="tool_tip" active="false" torquescale="1"/>
   </equality>
 </mujoco>
diff --git a/src/factory_sim/description/scene.xml b/src/factory_sim/description/scene.xml
index 43b28229..ae08b3c2 100644
--- a/src/factory_sim/description/scene.xml
+++ b/src/factory_sim/description/scene.xml
@@ -1,85 +1,46 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="fanuc scene">
-  <compiler angle="radian" autolimits="true" />
-  <option integrator="implicit" />
+  <compiler angle="radian" autolimits="true"/>
+  <option integrator="implicit"/>
 
   <asset>
     <!-- Define textures and materials -->
-    <texture name="wood" type="cube" file="assets/wood.png" />
-    <material name="wood" texture="wood" texrepeat="1 1" />
-    <mesh name="bowl" file="assets/bowl.stl" />
+    <texture name="wood" type="cube" file="assets/wood.png"/>
+    <material name="wood" texture="wood" texrepeat="1 1"/>
+    <mesh name="bowl" file="assets/bowl.stl"/>
 
-    <texture
-      type="skybox"
-      builtin="gradient"
-      rgb1="0.3 0.5 0.7"
-      rgb2="0 0 0"
-      width="512"
-      height="3072"
-    />
-    <texture
-      type="2d"
-      name="groundplane"
-      builtin="checker"
-      mark="edge"
-      rgb1="0.2 0.3 0.4"
-      rgb2="0.1 0.2 0.3"
-      markrgb="0.8 0.8 0.8"
-      width="300"
-      height="300"
-    />
-    <material
-      name="groundplane"
-      texture="groundplane"
-      texuniform="true"
-      texrepeat="5 5"
-      reflectance="0.2"
-    />
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
   </asset>
 
-  <statistic center="0.3 0 0.4" extent="0.8" />
+  <statistic center="0.3 0 0.4" extent="0.8"/>
 
   <visual>
-    <headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0 0 0" />
-    <rgba haze="0.15 0.25 0.35 1" />
-    <global azimuth="120" elevation="-20" />
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0 0 0"/>
+    <rgba haze="0.15 0.25 0.35 1"/>
+    <global azimuth="120" elevation="-20"/>
   </visual>
 
   <!-- Add Fanuc support files -->
-  <include file="lrmate200id/lrmate200id_globals.xml" />
+  <include file="lrmate200id/lrmate200id_globals.xml"/>
 
   <worldbody>
-    <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
-    <geom
-      name="floor"
-      pos="0 0 0"
-      size="0 0 0.05"
-      type="plane"
-      material="groundplane"
-    />
+    <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
+    <geom name="floor" pos="0 0 0" size="0 0 0.05" type="plane" material="groundplane"/>
     <!-- Add a scene camera -->
-    <camera
-      name="scene_camera"
-      pos="-1.15 0.000 1.900"
-      fovy="58"
-      mode="fixed"
-      resolution="640 480"
-      xyaxes="0.000 -1.000 0.000 0.713 0.000 0.701"
-    />
-    <site
-      name="scene_camera_optical_frame"
-      pos="-1.15 0.000 1.900"
-      xyaxes="0.000 -1.000 0.000 -0.713 -0.000 -0.701"
-    />
+    <camera name="scene_camera" pos="-1.15 0.000 1.900" fovy="58" mode="fixed" resolution="640 480" xyaxes="0.000 -1.000 0.000 0.713 0.000 0.701"/>
+    <site name="scene_camera_optical_frame" pos="-1.15 0.000 1.900" xyaxes="0.000 -1.000 0.000 -0.713 -0.000 -0.701"/>
 
     <body name="arm_pedestal" pos="0.0 0.0 0.15">
-      <geom type="box" size="0.2 0.2 0.15" />
-      <geom type="box" size="0.06 0.01 0.01" pos="0.23 0.03 0.1" />
-      <geom type="box" size="0.06 0.01 0.01" pos="0.23 -0.03 0.1" />
-      <site name="tool_holder_site" pos="0.25 0 0.15" euler="3.1415 0 0" />
+      <geom type="box" size="0.2 0.2 0.15"/>
+      <geom type="box" size="0.06 0.01 0.01" pos="0.23 0.03 0.1"/>
+      <geom type="box" size="0.06 0.01 0.01" pos="0.23 -0.03 0.1"/>
+      <site name="tool_holder_site" pos="0.25 0 0.15" euler="3.1415 0 0"/>
     </body>
 
     <body name="arm_mount" pos="0.0 0.0 0.3">
-      <include file="lrmate200id/lrmate200id.xml" />
+      <include file="lrmate200id/lrmate200id.xml"/>
     </body>
 
     <!--  -->
@@ -87,7 +48,7 @@
     <!--  -->
     <!-- Define the table for bin picking scenario -->
     <body name="bin_table" pos="0 0.6 0.2">
-      <geom type="box" size="0.6 0.4 0.025" rgba="0.1 0.1 0.1 1" />
+      <geom type="box" size="0.6 0.4 0.025" rgba="0.1 0.1 0.1 1"/>
 
       <body name="left_bin" pos="-0.25 0 0.025" quat="0.707 0 0 0.707">
         <!-- <geom type="box" size="1 1 .01" pos="0.5 0.5 0" rgba=".5 .5 .5 1"/>
@@ -96,84 +57,34 @@
         <!-- <box size="${x_length} ${y_length} ${bottom_thickness}"/> -->
         <!-- <origin xyz="${x_length/2} ${y_length/2} ${bottom_thickness/2}"/> -->
         <!-- <geom type="box" size="0.3048 0.1905 0.0025" pos="0 0 0.0025" rgba=".11 .51 .86 1"/> -->
-        <geom
-          type="box"
-          size="0.3048 0.1905 0.0025"
-          pos="0 0 0.0025"
-          rgba=".11 .51 .86 1"
-        />
+        <geom type="box" size="0.3048 0.1905 0.0025" pos="0 0 0.0025" rgba=".11 .51 .86 1"/>
         <!-- Back -->
         <!-- <box size="${side_wall_thickness} ${y_length-side_wall_thickness} ${z_length}"/> -->
         <!-- <origin xyz="${side_wall_thickness/2} ${(y_length-side_wall_thickness)/2} ${z_length/2}"/> -->
         <!-- <geom type="box" size="0.0125 0.190 0.09525" pos="0.3048-0.0125 0 0.0025+0.09525" rgba="1 .5 .5 1"/> -->
-        <geom
-          type="box"
-          size="0.0125 0.1655 0.09525"
-          pos="0.2923 0 0.09775"
-          rgba=".11 .51 .86 1"
-        />
+        <geom type="box" size="0.0125 0.1655 0.09525" pos="0.2923 0 0.09775" rgba=".11 .51 .86 1"/>
         <!-- Front -->
         <!-- <box size="${side_wall_thickness} ${y_length} ${z_length}"/> -->
         <!-- <origin xyz="${x_length - side_wall_thickness/2} ${y_length/2} ${z_length/2}"/> -->
         <!-- <geom type="box" size="0.0125 0.190 0.09525" pos="-0.3048+0.0125 0 0.0025+0.09525" rgba="1 .5 .5 1"/> -->
-        <geom
-          type="box"
-          size="0.0125 0.1655 0.09525"
-          pos="-0.2923 0 0.09775"
-          rgba=".11 .51 .86 1"
-        />
+        <geom type="box" size="0.0125 0.1655 0.09525" pos="-0.2923 0 0.09775" rgba=".11 .51 .86 1"/>
         <!-- Right -->
         <!-- <box size="${x_length} ${side_wall_thickness} ${z_length}"/> -->
         <!-- <origin xyz="${x_length/2} ${side_wall_thickness/2} ${z_length/2}"/> -->
         <!-- <geom type="box" size="0.3048 0.0125 0.09525" pos="0 0.1905-0.0125 0.09775" rgba=".5 .5 .5 1"/> -->
-        <geom
-          type="box"
-          size="0.3048 0.0125 0.09525"
-          pos="0 0.178 0.09775"
-          rgba=".11 .51 .86 1"
-        />
+        <geom type="box" size="0.3048 0.0125 0.09525" pos="0 0.178 0.09775" rgba=".11 .51 .86 1"/>
         <!-- Left -->
         <!-- <box size="${x_length} ${side_wall_thickness} ${z_length}"/> -->
         <!-- <origin xyz="${x_length/2} ${y_length - side_wall_thickness/2} ${z_length/2}"/> -->
-        <geom
-          type="box"
-          size="0.3048 0.0125 0.09525"
-          pos="0 -0.178 0.09775"
-          rgba=".11 .51 .86 1"
-        />
+        <geom type="box" size="0.3048 0.0125 0.09525" pos="0 -0.178 0.09775" rgba=".11 .51 .86 1"/>
       </body>
 
       <body name="right_bin" pos="0.25 0 0.025" quat="0.707 0 0 0.707">
-        <geom
-          type="box"
-          size="0.3048 0.1905 0.0025"
-          pos="0 0 0.0025"
-          rgba=".11 .51 .86 1"
-        />
-        <geom
-          type="box"
-          size="0.0125 0.1655 0.09525"
-          pos="0.2923 0 0.09775"
-          rgba=".11 .51 .86 1"
-        />
-        <geom
-          type="box"
-          size="0.0125 0.1655 0.09525"
-          pos="-0.2923 0 0.09775"
-          rgba=".11 .51 .86 1"
-        />
-        <geom
-          type="box"
-          size="0.3048 0.0125 0.09525"
-          pos="0 0.178 0.09775"
-          rgba=".11 .51 .86 1"
-        />
-        <geom
-          type="box"
-          size="0.3048 0.0125 0.09525"
-          pos="0 -0.178 0.09775"
-          rgba=".11 .51 .86 1"
-        />
+        <geom type="box" size="0.3048 0.1905 0.0025" pos="0 0 0.0025" rgba=".11 .51 .86 1"/>
+        <geom type="box" size="0.0125 0.1655 0.09525" pos="0.2923 0 0.09775" rgba=".11 .51 .86 1"/>
+        <geom type="box" size="0.0125 0.1655 0.09525" pos="-0.2923 0 0.09775" rgba=".11 .51 .86 1"/>
+        <geom type="box" size="0.3048 0.0125 0.09525" pos="0 0.178 0.09775" rgba=".11 .51 .86 1"/>
+        <geom type="box" size="0.3048 0.0125 0.09525" pos="0 -0.178 0.09775" rgba=".11 .51 .86 1"/>
       </body>
     </body>
 
@@ -183,16 +94,10 @@
       <replicate count="6" sep="-" offset="0 .07 0">
         <replicate count="2" sep="-" offset="0 0 0.5">
           <body name="sphere-0" pos="-0.35 0.4 0.5">
-            <joint type="slide" axis="1 0 0" group="3" />
-            <joint type="slide" axis="0 1 0" group="3" />
-            <joint type="slide" axis="0 0 1" group="3" />
-            <geom
-              type="box"
-              size="0.025 0.025 0.025"
-              condim="1"
-              priority="1"
-              material="wood"
-            />
+            <joint type="slide" axis="1 0 0" group="3"/>
+            <joint type="slide" axis="0 1 0" group="3"/>
+            <joint type="slide" axis="0 0 1" group="3"/>
+            <geom type="box" size="0.025 0.025 0.025" condim="1" priority="1" material="wood"/>
           </body>
         </replicate>
       </replicate>
@@ -204,16 +109,10 @@
       <replicate count="6" sep="-" offset="0 .07 0">
         <replicate count="2" sep="-" offset="0 0 0.5">
           <body name="sphere-1" pos="-0.3 0.45 0.75">
-            <joint type="slide" axis="1 0 0" group="3" />
-            <joint type="slide" axis="0 1 0" group="3" />
-            <joint type="slide" axis="0 0 1" group="3" />
-            <geom
-              type="box"
-              size="0.025 0.025 0.025"
-              condim="1"
-              priority="1"
-              material="wood"
-            />
+            <joint type="slide" axis="1 0 0" group="3"/>
+            <joint type="slide" axis="0 1 0" group="3"/>
+            <joint type="slide" axis="0 0 1" group="3"/>
+            <geom type="box" size="0.025 0.025 0.025" condim="1" priority="1" material="wood"/>
           </body>
         </replicate>
       </replicate>
@@ -225,43 +124,43 @@
     <!-- Define the table for assembly scenario -->
     <!-- y -0.2-0.2 -->
     <body name="assembly_table" pos="0 -0.4 0.2">
-      <geom type="box" size="0.6 0.2 0.025" rgba="0.1 0.1 0.1 1" />
+      <geom type="box" size="0.6 0.2 0.025" rgba="0.1 0.1 0.1 1"/>
     </body>
     <!-- z 0.225+.005+.0001 -->
     <body name="shelf" pos="0.25 -0.4 0.2301">
-      <geom type="box" size="0.2 0.1 0.005" material="wood" />
-      <freejoint />
+      <geom type="box" size="0.2 0.1 0.005" material="wood"/>
+      <freejoint/>
     </body>
     <!-- x -0.05-0.025 -->
     <!-- z 0.225+.05+.0001 -->
     <body name="stud1" pos="-0.075 -0.4 0.2751">
-      <geom type="box" size="0.025 0.1 0.05" material="wood" />
-      <freejoint />
+      <geom type="box" size="0.025 0.1 0.05" material="wood"/>
+      <freejoint/>
     </body>
     <!-- x -0.45+0.025 -->
     <!-- z 0.225+.05+.0001 -->
     <body name="stud2" pos="-0.425 -0.4 0.2751">
-      <geom type="box" size="0.025 0.1 0.05" material="wood" />
-      <freejoint />
+      <geom type="box" size="0.025 0.1 0.05" material="wood"/>
+      <freejoint/>
     </body>
 
     <!-- Left stud jig -->
     <!-- x -0.45-0.01-0.0001 -->
     <!-- z 0.225+.025 -->
     <body name="jig1_1" pos="-0.4601 -0.4 0.25">
-      <geom type="cylinder" size="0.01 0.025" rgba="0.1 0.1 0.1 1" />
+      <geom type="cylinder" size="0.01 0.025" rgba="0.1 0.1 0.1 1"/>
     </body>
     <!-- x -0.4+0.01+0.0001 -->
     <body name="jig1_2" pos="-0.3899 -0.4 0.25">
-      <geom type="cylinder" size="0.01 0.025" rgba="0.1 0.1 0.1 1" />
+      <geom type="cylinder" size="0.01 0.025" rgba="0.1 0.1 0.1 1"/>
     </body>
     <!-- y -0.5-0.01-0.0001 -->
     <body name="jig1_3" pos="-0.425 -0.5101 0.25">
-      <geom type="cylinder" size="0.01 0.025" rgba="0.1 0.1 0.1 1" />
+      <geom type="cylinder" size="0.01 0.025" rgba="0.1 0.1 0.1 1"/>
     </body>
     <!-- y -0.3+0.01+0.0001 -->
     <body name="jig1_4" pos="-0.425 -0.2899 0.25">
-      <geom type="cylinder" size="0.01 0.025" rgba="0.1 0.1 0.1 1" />
+      <geom type="cylinder" size="0.01 0.025" rgba="0.1 0.1 0.1 1"/>
     </body>
 
     <!-- Right stud jig -->
@@ -271,117 +170,42 @@
     <!-- REGISTER AND RASTERIZE SCENARIO -->
     <!--  -->
     <body name="stationary_rail" pos="1.1 0 0">
-      <geom
-        type="box"
-        size="0.04 0.04 0.7"
-        pos="0 0.8 0.7"
-        rgba="0.21 0.88 0.55 1"
-      />
-      <geom
-        type="box"
-        size="0.24 0.04 0.04"
-        pos="-0.2 0.8 1.44"
-        rgba="0.21 0.88 0.55 1"
-      />
+      <geom type="box" size="0.04 0.04 0.7" pos="0 0.8 0.7" rgba="0.21 0.88 0.55 1"/>
+      <geom type="box" size="0.24 0.04 0.04" pos="-0.2 0.8 1.44" rgba="0.21 0.88 0.55 1"/>
 
-      <geom
-        type="box"
-        size="0.04 0.04 0.7"
-        pos="0 -0.8 0.7"
-        rgba="0.21 0.88 0.55 1"
-      />
-      <geom
-        type="box"
-        size="0.24 0.04 0.04"
-        pos="-0.2 -0.8 1.44"
-        rgba="0.21 0.88 0.55 1"
-      />
+      <geom type="box" size="0.04 0.04 0.7" pos="0 -0.8 0.7" rgba="0.21 0.88 0.55 1"/>
+      <geom type="box" size="0.24 0.04 0.04" pos="-0.2 -0.8 1.44" rgba="0.21 0.88 0.55 1"/>
 
-      <geom
-        type="box"
-        size="0.04 1.0 0.04"
-        pos="-0.4 0.0 1.36"
-        rgba="0.8 0.8 0.8 1"
-      />
+      <geom type="box" size="0.04 1.0 0.04" pos="-0.4 0.0 1.36" rgba="0.8 0.8 0.8 1"/>
 
-      <geom
-        type="mesh"
-        mesh="bowl"
-        pos="-0.45 0.6 0.9"
-        euler="0 1.5708 0"
-        material="wood"
-      />
-      <site name="convex_bowl_1" pos="-0.45 0.6 0.9" euler="0 0 1.5708" />
-      <geom
-        type="box"
-        size="0.01 0.02 0.16"
-        pos="-0.4 0.6 1.22"
-        rgba="0.1 0.1 0.1 1"
-      />
+      <geom type="mesh" mesh="bowl" pos="-0.45 0.6 0.9" euler="0 1.5708 0" material="wood"/>
+      <site name="convex_bowl_1" pos="-0.45 0.6 0.9" euler="0 0 1.5708"/>
+      <geom type="box" size="0.01 0.02 0.16" pos="-0.4 0.6 1.22" rgba="0.1 0.1 0.1 1"/>
 
-      <geom
-        type="mesh"
-        mesh="bowl"
-        pos="-0.45 0.2 0.9"
-        euler="0 1.5708 0"
-        material="wood"
-      />
-      <site name="convex_bowl_2" pos="-0.45 0.2 0.9" euler="0 0 1.5708" />
-      <geom
-        type="box"
-        size="0.01 0.02 0.16"
-        pos="-0.4 0.2 1.22"
-        rgba="0.1 0.1 0.1 1"
-      />
+      <geom type="mesh" mesh="bowl" pos="-0.45 0.2 0.9" euler="0 1.5708 0" material="wood"/>
+      <site name="convex_bowl_2" pos="-0.45 0.2 0.9" euler="0 0 1.5708"/>
+      <geom type="box" size="0.01 0.02 0.16" pos="-0.4 0.2 1.22" rgba="0.1 0.1 0.1 1"/>
 
-      <geom
-        type="mesh"
-        mesh="bowl"
-        pos="-0.35 -0.2 0.9"
-        euler="0 -1.5708 0"
-        material="wood"
-      />
-      <site name="concave_bowl_1" pos="-0.456 -0.2 0.9" euler="0 0 -1.5708" />
-      <geom
-        type="box"
-        size="0.01 0.02 0.16"
-        pos="-0.4 -0.2 1.22"
-        rgba="0.1 0.1 0.1 1"
-      />
+      <geom type="mesh" mesh="bowl" pos="-0.35 -0.2 0.9" euler="0 -1.5708 0" material="wood"/>
+      <site name="concave_bowl_1" pos="-0.456 -0.2 0.9" euler="0 0 -1.5708"/>
+      <geom type="box" size="0.01 0.02 0.16" pos="-0.4 -0.2 1.22" rgba="0.1 0.1 0.1 1"/>
 
-      <geom
-        type="mesh"
-        mesh="bowl"
-        pos="-0.35 -0.6 0.9"
-        euler="0 -1.5708 0"
-        material="wood"
-      />
-      <site name="concave_bowl_2" pos="-0.456 -0.6 0.9" euler="0 0 -1.5708" />
-      <geom
-        type="box"
-        size="0.01 0.02 0.16"
-        pos="-0.4 -0.6 1.22"
-        rgba="0.1 0.1 0.1 1"
-      />
+      <geom type="mesh" mesh="bowl" pos="-0.35 -0.6 0.9" euler="0 -1.5708 0" material="wood"/>
+      <site name="concave_bowl_2" pos="-0.456 -0.6 0.9" euler="0 0 -1.5708"/>
+      <geom type="box" size="0.01 0.02 0.16" pos="-0.4 -0.6 1.22" rgba="0.1 0.1 0.1 1"/>
     </body>
   </worldbody>
 
   <!-- Add tool -->
-  <include file="tool.xml" />
+  <include file="tool.xml"/>
 
   <!-- Define weld constraints between the robot flange (tool0) and the gripper -->
   <equality>
-    <weld
-      name="tool_attachment"
-      body1="tool0"
-      body2="gripper_base"
-      active="false"
-      torquescale="1"
-    />
+    <weld name="tool_attachment" body1="tool0" body2="gripper_base" active="false" torquescale="1"/>
   </equality>
 
   <!-- Add welds for suction gripping of cubes -->
-  <include file="box-welds.xml" />
+  <include file="box-welds.xml"/>
 
   <!-- TODO Add welds for suction gripping of assembly parts -->
 </mujoco>
diff --git a/src/factory_sim/description/tool.xml b/src/factory_sim/description/tool.xml
index 9ee014e5..2699c101 100644
--- a/src/factory_sim/description/tool.xml
+++ b/src/factory_sim/description/tool.xml
@@ -1,39 +1,16 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="fanuc_tool">
   <worldbody>
-    <body
-      name="gripper_base"
-      gravcomp="1"
-      pos="0.25 0.0 0.3"
-      euler="3.1415 0 0"
-    >
-      <joint
-        type="free"
-        stiffness="0"
-        damping="0"
-        frictionloss=".001"
-        armature="0"
-      />
-      <site name="tool_attach_site" pos="0 0 0" euler="0 0 0" />
+    <body name="gripper_base" gravcomp="1" pos="0.25 0.0 0.3" euler="3.1415 0 0">
+      <joint type="free" stiffness="0" damping="0" frictionloss=".001" armature="0"/>
+      <site name="tool_attach_site" pos="0 0 0" euler="0 0 0"/>
 
-      <geom type="cylinder" size="0.03 0.02" pos="0 0 0.02" rgba=".1 .1 .1 1" />
+      <geom type="cylinder" size="0.03 0.02" pos="0 0 0.02" rgba=".1 .1 .1 1"/>
       <body name="suction_cylinder" gravcomp="1" pos="0 0 0.04">
-        <geom
-          type="cylinder"
-          size="0.01 0.03"
-          pos="0 0 0.03"
-          margin="0.01"
-          gap="0.01"
-          rgba=".1 .1 .1 1"
-        />
+        <geom type="cylinder" size="0.01 0.03" pos="0 0 0.03" margin="0.01" gap="0.01" rgba=".1 .1 .1 1"/>
         <body name="tool_tip" gravcomp="1" pos="0 0 0.055">
-          <geom
-            class="collision"
-            type="cylinder"
-            size="0.01 0.005"
-            margin="0.01"
-            gap="0.01"
-          />
-          <site name="suction_cup_tool_tip" />
+          <geom class="collision" type="cylinder" size="0.01 0.005" margin="0.01" gap="0.01"/>
+          <site name="suction_cup_tool_tip"/>
         </body>
       </body>
     </body>
diff --git a/src/factory_sim/launch/agent_bridge.launch.xml b/src/factory_sim/launch/agent_bridge.launch.xml
index b2aa7543..8eeec7d9 100644
--- a/src/factory_sim/launch/agent_bridge.launch.xml
+++ b/src/factory_sim/launch/agent_bridge.launch.xml
@@ -1,6 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
 </launch>
diff --git a/src/factory_sim/objectives/activate_vacuum.xml b/src/factory_sim/objectives/activate_vacuum.xml
index bce34480..917bba2d 100644
--- a/src/factory_sim/objectives/activate_vacuum.xml
+++ b/src/factory_sim/objectives/activate_vacuum.xml
@@ -1,19 +1,14 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Activate Vacuum">
   <BehaviorTree ID="Activate Vacuum" _description="Activate the vacuum gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        position="1"
-        gripper_command_action_name="/suction_cup_controller/gripper_cmd"
-        timeout="10.000000"
-      />
+      <Action ID="MoveGripperAction" position="1" gripper_command_action_name="/suction_cup_controller/gripper_cmd" timeout="10.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Activate Vacuum">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/clear_snapshot.xml b/src/factory_sim/objectives/clear_snapshot.xml
index 5ac3f027..ee491e78 100644
--- a/src/factory_sim/objectives/clear_snapshot.xml
+++ b/src/factory_sim/objectives/clear_snapshot.xml
@@ -1,16 +1,13 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Clear Snapshot">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Clear Snapshot"
-    _description="Clear the point cloud snapshot"
-  >
-    <Action ID="ClearSnapshot" />
+  <BehaviorTree ID="Clear Snapshot" _description="Clear the point cloud snapshot">
+    <Action ID="ClearSnapshot"/>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Clear Snapshot">
       <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
+        <Metadata subcategory="Perception - 3D Point Cloud"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/deactivate_vacuum.xml b/src/factory_sim/objectives/deactivate_vacuum.xml
index 43ca5213..493a81e3 100644
--- a/src/factory_sim/objectives/deactivate_vacuum.xml
+++ b/src/factory_sim/objectives/deactivate_vacuum.xml
@@ -1,22 +1,15 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Deactivate Vacuum">
   <!--//////////-->
-  <BehaviorTree
-    ID="Deactivate Vacuum"
-    _description="Deactivate the vacuum gripper"
-  >
+  <BehaviorTree ID="Deactivate Vacuum" _description="Deactivate the vacuum gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        position="0"
-        gripper_command_action_name="/suction_cup_controller/gripper_cmd"
-      />
+      <Action ID="MoveGripperAction" position="0" gripper_command_action_name="/suction_cup_controller/gripper_cmd"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Deactivate Vacuum">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/interpolate_to_joint_state.xml b/src/factory_sim/objectives/interpolate_to_joint_state.xml
index 30942c4d..f9b271bf 100644
--- a/src/factory_sim/objectives/interpolate_to_joint_state.xml
+++ b/src/factory_sim/objectives/interpolate_to_joint_state.xml
@@ -1,42 +1,20 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Interpolate to Joint State">
-  <BehaviorTree
-    ID="Interpolate to Joint State"
-    _description="Move to a specified joint state using joint interpolation"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Interpolate to Joint State" _description="Move to a specified joint state using joint interpolation" _subtreeOnly="true">
     <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="interpolate_to_joint_state"
-        controller_names="/joint_trajectory_controller"
-        task="{interpolate_to_waypoint_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}" />
-      <Action
-        ID="SetupMTCInterpolateToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCInterpolateToJointState_First"
-        planning_group_name="manipulator"
-        task="{interpolate_to_waypoint_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{interpolate_to_waypoint_solution}"
-        task="{interpolate_to_waypoint_task}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{interpolate_to_waypoint_solution}"
-      />
+      <Action ID="InitializeMTCTask" task_id="interpolate_to_joint_state" controller_names="/joint_trajectory_controller" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="SetupMTCInterpolateToJointState" joint_state="{target_joint_state}" name="SetupMTCInterpolateToJointState_First" planning_group_name="manipulator" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="PlanMTCTask" solution="{interpolate_to_waypoint_solution}" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="ExecuteMTCTask" solution="{interpolate_to_waypoint_solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Interpolate to Joint State">
-      <input_port name="target_joint_state" default="{target_joint_state}" />
+      <input_port name="target_joint_state" default="{target_joint_state}"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/move_to_joint_state.xml b/src/factory_sim/objectives/move_to_joint_state.xml
index b6caa0ea..4f2e656d 100644
--- a/src/factory_sim/objectives/move_to_joint_state.xml
+++ b/src/factory_sim/objectives/move_to_joint_state.xml
@@ -1,48 +1,26 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
-  <BehaviorTree
-    ID="Move to Joint State"
-    _description="Move to a specified joint state"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Move to Joint State" _description="Move to a specified joint state" _subtreeOnly="true">
     <Control ID="Sequence">
-      <Action ID="ActivateControllers" controller_names="{controller_names}" />
-      <Action
-        ID="PlanToJointGoal"
-        joint_goal="{target_joint_state}"
-        keep_orientation_tolerance="{keep_orientation_tolerance}"
-        link_padding="{link_padding}"
-        keep_orientation="{keep_orientation}"
-        acceleration_scale_factor="{acceleration_scale_factor}"
-        velocity_scale_factor="{velocity_scale_factor}"
-        planning_group_name="{joint_group_name}"
-      />
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="{controller_action_server}"
-      />
+      <Action ID="ActivateControllers" controller_names="{controller_names}"/>
+      <Action ID="PlanToJointGoal" joint_goal="{target_joint_state}" keep_orientation_tolerance="{keep_orientation_tolerance}" link_padding="{link_padding}" keep_orientation="{keep_orientation}" acceleration_scale_factor="{acceleration_scale_factor}" velocity_scale_factor="{velocity_scale_factor}" planning_group_name="{joint_group_name}"/>
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="{controller_action_server}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Joint State">
-      <input_port name="target_joint_state" default="{target_joint_state}" />
-      <input_port name="joint_group_name" default="manipulator" />
-      <input_port name="link_padding" default="0.01" />
-      <input_port name="keep_orientation_tolerance" default="0.05" />
-      <input_port name="keep_orientation" default="false" />
-      <input_port name="velocity_scale_factor" default="1.0" />
-      <input_port name="acceleration_scale_factor" default="1.0" />
-      <input_port
-        name="controller_names"
-        default="joint_trajectory_controller"
-      />
-      <input_port
-        name="controller_action_server"
-        default="/joint_trajectory_controller/follow_joint_trajectory"
-      />
+      <input_port name="target_joint_state" default="{target_joint_state}"/>
+      <input_port name="joint_group_name" default="manipulator"/>
+      <input_port name="link_padding" default="0.01"/>
+      <input_port name="keep_orientation_tolerance" default="0.05"/>
+      <input_port name="keep_orientation" default="false"/>
+      <input_port name="velocity_scale_factor" default="1.0"/>
+      <input_port name="acceleration_scale_factor" default="1.0"/>
+      <input_port name="controller_names" default="joint_trajectory_controller"/>
+      <input_port name="controller_action_server" default="/joint_trajectory_controller/follow_joint_trajectory"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/move_to_pose.xml b/src/factory_sim/objectives/move_to_pose.xml
index 75dd99a1..4f6b94cf 100644
--- a/src/factory_sim/objectives/move_to_pose.xml
+++ b/src/factory_sim/objectives/move_to_pose.xml
@@ -1,43 +1,21 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Pose">
-  <BehaviorTree
-    ID="Move to Pose"
-    _description="Plan and execute motion to a Cartesian coordinate pose in a ROS message type, using inverse kinematics."
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Move to Pose" _description="Plan and execute motion to a Cartesian coordinate pose in a ROS message type, using inverse kinematics." _subtreeOnly="true">
     <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_pose"
-        controller_names="/joint_trajectory_controller"
-        task="{move_to_pose_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
-      <Action
-        ID="SetupMTCPlanToPose"
-        ik_frame="tool0"
-        planning_group_name="manipulator"
-        target_pose="{target_pose}"
-        task="{move_to_pose_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_pose_solution}"
-        task="{move_to_pose_task}"
-      />
-      <SubTree
-        ID="Wait for Trajectory Approval if User Available"
-        solution="{move_to_pose_solution}"
-      />
-      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
+      <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCPlanToPose" ik_frame="tool0" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}"/>
+      <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+      <SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Pose">
-      <input_port name="target_pose" default="{target_pose}" />
+      <input_port name="target_pose" default="{target_pose}"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/move_to_waypoint.xml b/src/factory_sim/objectives/move_to_waypoint.xml
index cac49208..183323e2 100644
--- a/src/factory_sim/objectives/move_to_waypoint.xml
+++ b/src/factory_sim/objectives/move_to_waypoint.xml
@@ -1,60 +1,25 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
-  <BehaviorTree
-    ID="Move to Waypoint"
-    _description="This Objective is used when moving to one of the saved waypoints in your site configuration"
-    _subtreeOnly="true"
-    acceleration_scale_factor="1.0"
-    controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-    controller_names="/joint_trajectory_controller"
-    joint_group_name="manipulator"
-    keep_orientation="false"
-    keep_orientation_tolerance="0.05"
-    link_padding="0.01"
-    velocity_scale_factor="1.0"
-  >
+  <BehaviorTree ID="Move to Waypoint" _description="This Objective is used when moving to one of the saved waypoints in your site configuration" _subtreeOnly="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="RetrieveWaypoint"
-        waypoint_joint_state="{target_joint_state}"
-        waypoint_name="{waypoint_name}"
-        joint_group_name="{joint_group_name}"
-      />
-      <SubTree
-        ID="Move to Joint State"
-        _collapsed="true"
-        velocity_scale_factor="{velocity_scale_factor}"
-        acceleration_scale_factor="{acceleration_scale_factor}"
-        link_padding="{link_padding}"
-        keep_orientation_tolerance="{keep_orientation_tolerance}"
-        keep_orientation="{keep_orientation}"
-        target_joint_state="{target_joint_state}"
-        controller_names="{controller_names}"
-        joint_group_name="{joint_group_name}"
-        controller_action_server="{controller_action_server}"
-      />
+      <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}" joint_group_name="{joint_group_name}"/>
+      <SubTree ID="Move to Joint State" _collapsed="true" velocity_scale_factor="{velocity_scale_factor}" acceleration_scale_factor="{acceleration_scale_factor}" link_padding="{link_padding}" keep_orientation_tolerance="{keep_orientation_tolerance}" keep_orientation="{keep_orientation}" target_joint_state="{target_joint_state}" controller_names="{controller_names}" joint_group_name="{joint_group_name}" controller_action_server="{controller_action_server}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Waypoint">
-      <input_port name="acceleration_scale_factor" default="1.0" />
-      <input_port
-        name="controller_action_server"
-        default="/joint_trajectory_controller/follow_joint_trajectory"
-      />
-      <input_port
-        name="controller_names"
-        default="/joint_trajectory_controller"
-      />
-      <input_port name="joint_group_name" default="manipulator" />
-      <input_port name="keep_orientation" default="false" />
-      <input_port name="keep_orientation_tolerance" default="0.05" />
-      <input_port name="link_padding" default="0.01" />
-      <input_port name="velocity_scale_factor" default="1.0" />
-      <input_port name="waypoint_name" default="" />
+      <input_port name="acceleration_scale_factor" default="1.0"/>
+      <input_port name="controller_action_server" default="/joint_trajectory_controller/follow_joint_trajectory"/>
+      <input_port name="controller_names" default="/joint_trajectory_controller"/>
+      <input_port name="joint_group_name" default="manipulator"/>
+      <input_port name="keep_orientation" default="false"/>
+      <input_port name="keep_orientation_tolerance" default="0.05"/>
+      <input_port name="link_padding" default="0.01"/>
+      <input_port name="velocity_scale_factor" default="1.0"/>
+      <input_port name="waypoint_name" default=""/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/pick_up_tool.xml b/src/factory_sim/objectives/pick_up_tool.xml
index f6c51d82..e2fe796d 100644
--- a/src/factory_sim/objectives/pick_up_tool.xml
+++ b/src/factory_sim/objectives/pick_up_tool.xml
@@ -1,99 +1,32 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick Up Tool">
   <!--//////////-->
-  <BehaviorTree
-    ID="Pick Up Tool"
-    _description="Pick up the tool from the tool holder"
-  >
+  <BehaviorTree ID="Pick Up Tool" _description="Pick up the tool from the tool holder">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="tool_attach_site"
-        position_xyz="0;0;-0.1"
-        stamped_pose="{approach_pose}"
-      />
-      <Action
-        ID="InitializeMTCTask"
-        task_id="pick_up_tool"
-        controller_names="/joint_trajectory_controller"
-        task="{mtc_task}"
-      />
-      <Action
-        ID="SetupMTCCurrentState"
-        skip_collision_check="false"
-        task="{mtc_task}"
-      />
-      <Action
-        ID="SetupMTCPlanToPose"
-        acceleration_scale_factor="0.5"
-        ik_frame="tool0"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.100000"
-        link_padding="0.001000"
-        max_iterations="5000"
-        monitored_stage="current state"
-        planning_group_name="manipulator"
-        target_pose="{approach_pose}"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="0.5"
-        task="{mtc_task}"
-        keep_orientation_link_names="grasp_link"
-      />
-      <Action
-        ID="SetupMTCMoveAlongFrameAxis"
-        hand_frame="tool0"
-        max_distance="0.095"
-        min_distance="0.095"
-        axis_frame="tool0"
-        acceleration_scale="0.5"
-        axis_x="0.000000"
-        axis_y="0.000000"
-        axis_z="1.000000"
-        ignore_environment_collisions="false"
-        planning_group_name="manipulator"
-        velocity_scale="0.5"
-        task="{mtc_task}"
-      />
-      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
-      <Action
-        ID="ExecuteMTCTask"
-        goal_duration_tolerance="-1.000000"
-        solution="{mtc_solution}"
-      />
-      <Action
-        ID="MoveGripperAction"
-        position="1"
-        gripper_command_action_name="/tool_attach_controller/gripper_cmd"
-        timeout="10"
-      />
+      <Action ID="CreateStampedPose" reference_frame="tool_attach_site" position_xyz="0;0;-0.1" stamped_pose="{approach_pose}"/>
+      <Action ID="InitializeMTCTask" task_id="pick_up_tool" controller_names="/joint_trajectory_controller" task="{mtc_task}"/>
+      <Action ID="SetupMTCCurrentState" skip_collision_check="false" task="{mtc_task}"/>
+      <Action ID="SetupMTCPlanToPose" acceleration_scale_factor="0.5" ik_frame="tool0" keep_orientation="false" keep_orientation_tolerance="0.100000" link_padding="0.001000" max_iterations="5000" monitored_stage="current state" planning_group_name="manipulator" target_pose="{approach_pose}" trajectory_sampling_rate="100" velocity_scale_factor="0.5" task="{mtc_task}" keep_orientation_link_names="grasp_link"/>
+      <Action ID="SetupMTCMoveAlongFrameAxis" hand_frame="tool0" max_distance="0.095" min_distance="0.095" axis_frame="tool0" acceleration_scale="0.5" axis_x="0.000000" axis_y="0.000000" axis_z="1.000000" ignore_environment_collisions="false" planning_group_name="manipulator" velocity_scale="0.5" task="{mtc_task}"/>
+      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}"/>
+      <Action ID="ExecuteMTCTask" goal_duration_tolerance="-1.000000" solution="{mtc_solution}"/>
+      <Action ID="MoveGripperAction" position="1" gripper_command_action_name="/tool_attach_controller/gripper_cmd" timeout="10"/>
       <Control ID="Fallback">
-        <Action
-          ID="AttachTool"
-          parent_link_name="tool0"
-          tool_name="suction_gripper"
-        />
+        <Action ID="AttachTool" parent_link_name="tool0" tool_name="suction_gripper"/>
         <Control ID="Sequence">
-          <Action
-            ID="LogMessage"
-            log_level="warn"
-            message="Tool missing from planning scene - running &quot;Setup Initial Planning Scene&quot; with assumption it is in tool holder."
-          />
-          <SubTree ID="Setup Initial Planning Scene" _collapsed="true" />
-          <Action
-            ID="AttachTool"
-            parent_link_name="tool0"
-            tool_name="suction_gripper"
-          />
+          <Action ID="LogMessage" log_level="warn" message="Tool missing from planning scene - running &quot;Setup Initial Planning Scene&quot; with assumption it is in tool holder."/>
+          <SubTree ID="Setup Initial Planning Scene" _collapsed="true"/>
+          <Action ID="AttachTool" parent_link_name="tool0" tool_name="suction_gripper"/>
         </Control>
       </Control>
-      <SubTree ID="Retract" _collapsed="true" distance="0.35" />
+      <SubTree ID="Retract" _collapsed="true" distance="0.35"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Pick Up Tool">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Grasping"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/place_tool_in_tool_holder.xml b/src/factory_sim/objectives/place_tool_in_tool_holder.xml
index 5b1171e7..a2975b00 100644
--- a/src/factory_sim/objectives/place_tool_in_tool_holder.xml
+++ b/src/factory_sim/objectives/place_tool_in_tool_holder.xml
@@ -1,87 +1,27 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Place Tool in Tool Holder">
   <!--//////////-->
-  <BehaviorTree
-    ID="Place Tool in Tool Holder"
-    _description="Puts the tool back in the tool holder"
-  >
+  <BehaviorTree ID="Place Tool in Tool Holder" _description="Puts the tool back in the tool holder">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="tool_holder_site"
-        position_xyz="0;0;-0.15"
-        stamped_pose="{approach_pose}"
-      />
-      <Action
-        ID="InitializeMTCTask"
-        task_id="pick_up_tool"
-        controller_names="/joint_trajectory_controller"
-        task="{mtc_task}"
-      />
-      <Action
-        ID="SetupMTCCurrentState"
-        skip_collision_check="false"
-        task="{mtc_task}"
-      />
-      <Action
-        ID="SetupMTCIgnoreCollisionsBetweenObjects"
-        allow_collision="true"
-        task="{mtc_task}"
-        object_names="link_5;link_6;suction_gripper"
-      />
-      <Action
-        ID="SetupMTCPlanToPose"
-        acceleration_scale_factor="0.5"
-        ik_frame="tool0"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.100000"
-        link_padding="0.020000"
-        max_iterations="5000"
-        monitored_stage="current state"
-        planning_group_name="manipulator"
-        target_pose="{approach_pose}"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="0.5"
-        task="{mtc_task}"
-        keep_orientation_link_names="grasp_link"
-      />
-      <Action
-        ID="SetupMTCMoveAlongFrameAxis"
-        hand_frame="tool0"
-        max_distance="0.14"
-        axis_frame="tool0"
-        min_distance="0.14"
-        acceleration_scale="0.5"
-        axis_x="0.000000"
-        axis_y="0.000000"
-        axis_z="1.000000"
-        ignore_environment_collisions="false"
-        planning_group_name="manipulator"
-        velocity_scale="0.5"
-        task="{mtc_task}"
-      />
-      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
-      <Action
-        ID="ExecuteMTCTask"
-        goal_duration_tolerance="-1.000000"
-        solution="{mtc_solution}"
-      />
-      <Action ID="WaitForDuration" delay_duration="1" />
-      <Action
-        ID="MoveGripperAction"
-        position="0"
-        gripper_command_action_name="/tool_attach_controller/gripper_cmd"
-        timeout="10"
-      />
-      <Action ID="DetachTool" tool_name="suction_gripper" />
-      <SubTree ID="Retract" _collapsed="true" distance="0.35" />
+      <Action ID="CreateStampedPose" reference_frame="tool_holder_site" position_xyz="0;0;-0.15" stamped_pose="{approach_pose}"/>
+      <Action ID="InitializeMTCTask" task_id="pick_up_tool" controller_names="/joint_trajectory_controller" task="{mtc_task}"/>
+      <Action ID="SetupMTCCurrentState" skip_collision_check="false" task="{mtc_task}"/>
+      <Action ID="SetupMTCIgnoreCollisionsBetweenObjects" allow_collision="true" task="{mtc_task}" object_names="link_5;link_6;suction_gripper"/>
+      <Action ID="SetupMTCPlanToPose" acceleration_scale_factor="0.5" ik_frame="tool0" keep_orientation="false" keep_orientation_tolerance="0.100000" link_padding="0.020000" max_iterations="5000" monitored_stage="current state" planning_group_name="manipulator" target_pose="{approach_pose}" trajectory_sampling_rate="100" velocity_scale_factor="0.5" task="{mtc_task}" keep_orientation_link_names="grasp_link"/>
+      <Action ID="SetupMTCMoveAlongFrameAxis" hand_frame="tool0" max_distance="0.14" axis_frame="tool0" min_distance="0.14" acceleration_scale="0.5" axis_x="0.000000" axis_y="0.000000" axis_z="1.000000" ignore_environment_collisions="false" planning_group_name="manipulator" velocity_scale="0.5" task="{mtc_task}"/>
+      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}"/>
+      <Action ID="ExecuteMTCTask" goal_duration_tolerance="-1.000000" solution="{mtc_solution}"/>
+      <Action ID="WaitForDuration" delay_duration="1"/>
+      <Action ID="MoveGripperAction" position="0" gripper_command_action_name="/tool_attach_controller/gripper_cmd" timeout="10"/>
+      <Action ID="DetachTool" tool_name="suction_gripper"/>
+      <SubTree ID="Retract" _collapsed="true" distance="0.35"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Place Tool in Tool Holder">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Grasping" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/reachability_analysis_-_blocks.xml b/src/factory_sim/objectives/reachability_analysis_-_blocks.xml
index 2f03d18b..7d949d99 100644
--- a/src/factory_sim/objectives/reachability_analysis_-_blocks.xml
+++ b/src/factory_sim/objectives/reachability_analysis_-_blocks.xml
@@ -1,43 +1,16 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Reachability Analysis - blocks">
   <!--//////////-->
-  <BehaviorTree
-    ID="Reachability Analysis - blocks"
-    _description="An example of performing reachability analysis for an overhead picking task."
-    _favorite="true"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Reachability Analysis - blocks" _description="An example of performing reachability analysis for an overhead picking task." _favorite="true" _subtreeOnly="false">
     <Control ID="Sequence" name="">
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="world"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0.7071;0.7071;0;0.0004"
-        position_xyz="-0.35;0.4;0.288"
-      />
-      <SubTree
-        ID="Reachability Analysis for plane"
-        _collapsed="true"
-        tip_link="tool0"
-        planning_group_name="manipulator"
-        controller_name="/joint_trajectory_controller"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        reachability_origin_pose="{stamped_pose}"
-        starting_waypoint="View blocks"
-        pick_height="0.1"
-        column_spacing_translation="0;0.175;0"
-        execute_trajectories="true"
-        num_45_deg_rotations="0"
-        num_columns="2"
-        num_rows="2"
-        pick_height_translation="0;0;-0.1"
-        row_spacing_translation="0.3;0;0"
-      />
+      <Action ID="CreateStampedPose" reference_frame="world" stamped_pose="{stamped_pose}" orientation_xyzw="0.7071;0.7071;0;0.0004" position_xyz="-0.35;0.4;0.288"/>
+      <SubTree ID="Reachability Analysis for plane" _collapsed="true" tip_link="tool0" planning_group_name="manipulator" controller_name="/joint_trajectory_controller" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" reachability_origin_pose="{stamped_pose}" starting_waypoint="View blocks" pick_height="0.1" column_spacing_translation="0;0.175;0" execute_trajectories="true" num_45_deg_rotations="0" num_columns="2" num_rows="2" pick_height_translation="0;0;-0.1" row_spacing_translation="0.3;0;0"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Reachability Analysis - blocks">
       <MetadataFields>
-        <Metadata subcategory="Reachability" />
+        <Metadata subcategory="Reachability"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/reachability_analysis_-_bowls.xml b/src/factory_sim/objectives/reachability_analysis_-_bowls.xml
index 5a86733e..ec401179 100644
--- a/src/factory_sim/objectives/reachability_analysis_-_bowls.xml
+++ b/src/factory_sim/objectives/reachability_analysis_-_bowls.xml
@@ -1,31 +1,16 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Reachability Analysis - bowls">
   <!--//////////-->
-  <BehaviorTree
-    ID="Reachability Analysis - bowls"
-    _description="An example of performing reachability analysis for a object-normal task such as painting or sanding."
-    _favorite="true"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Reachability Analysis - bowls" _description="An example of performing reachability analysis for a object-normal task such as painting or sanding." _favorite="true" _subtreeOnly="false">
     <Control ID="Sequence" name="">
-      <SubTree
-        ID="Reachability Analysis for mesh"
-        _collapsed="true"
-        tip_link="tool0"
-        planning_group_name="manipulator"
-        controller_name="/joint_trajectory_controller"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        starting_waypoint="View bowls"
-        mesh_link_name="convex_bowl"
-        tip_offset="0;0;0;0;0;0"
-        execute_trajectories="true"
-      />
+      <SubTree ID="Reachability Analysis for mesh" _collapsed="true" tip_link="tool0" planning_group_name="manipulator" controller_name="/joint_trajectory_controller" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" starting_waypoint="View bowls" mesh_link_name="convex_bowl" tip_offset="0;0;0;0;0;0" execute_trajectories="true"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Reachability Analysis - bowls">
       <MetadataFields>
-        <Metadata subcategory="Reachability" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Reachability"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/reachability_analysis_for_mesh.xml b/src/factory_sim/objectives/reachability_analysis_for_mesh.xml
index 03e1a15c..382728f1 100644
--- a/src/factory_sim/objectives/reachability_analysis_for_mesh.xml
+++ b/src/factory_sim/objectives/reachability_analysis_for_mesh.xml
@@ -1,197 +1,65 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Reachability Analysis for mesh">
   <!--//////////-->
-  <BehaviorTree
-    ID="Reachability Analysis for mesh"
-    _description="Given a starting waypoint and mesh in the URDF attempt to Cartesian move from starting waypoint to each mesh normal."
-    controller_action_server="{controller_action_server}"
-    controller_name="{controller_name}"
-    execute_trajectories="true"
-    mesh_link_name="{mesh_link_name}"
-    planning_group_name="{planning_group_name}"
-    starting_waypoint="{starting_waypoint}"
-    tip_link="{tip_link}"
-    tip_offset="0;0;0;0;0;0"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Reachability Analysis for mesh" _description="Given a starting waypoint and mesh in the URDF attempt to Cartesian move from starting waypoint to each mesh normal." controller_action_server="{controller_action_server}" controller_name="{controller_name}" execute_trajectories="true" mesh_link_name="{mesh_link_name}" planning_group_name="{planning_group_name}" starting_waypoint="{starting_waypoint}" tip_link="{tip_link}" tip_offset="0;0;0;0;0;0" _subtreeOnly="true">
     <Control ID="Sequence">
       <Control ID="Sequence" name="Move to starting waypoint">
-        <Action
-          ID="RetrieveWaypoint"
-          joint_group_name="{planning_group_name}"
-          waypoint_joint_state="{starting_joint_state}"
-          waypoint_name="{starting_waypoint}"
-        />
-        <SubTree
-          ID="Move to Joint State"
-          _collapsed="false"
-          target_joint_state="{starting_joint_state}"
-          acceleration_scale_factor="1.0"
-          controller_action_server="{controller_action_server}"
-          controller_names="{controller_name}"
-          joint_group_name="{planning_group_name}"
-          keep_orientation="false"
-          keep_orientation_tolerance="0.05"
-          link_padding="0.01"
-          velocity_scale_factor="1.0"
-        />
+        <Action ID="RetrieveWaypoint" joint_group_name="{planning_group_name}" waypoint_joint_state="{starting_joint_state}" waypoint_name="{starting_waypoint}"/>
+        <SubTree ID="Move to Joint State" _collapsed="false" target_joint_state="{starting_joint_state}" acceleration_scale_factor="1.0" controller_action_server="{controller_action_server}" controller_names="{controller_name}" joint_group_name="{planning_group_name}" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0"/>
       </Control>
       <Control ID="Sequence" name="Make vector of place poses to plan to">
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="tool0"
-          stamped_pose="{tool_pose}"
-          orientation_xyzw="0;0;0;1"
-          position_xyz="0;0;0"
-        />
-        <Action
-          ID="VisualizePose"
-          marker_name="tool_pose"
-          pose="{tool_pose}"
-          marker_lifetime="0.000000"
-          marker_size="0.200000"
-        />
-        <Action
-          ID="GetMeshNormalPoses"
-          discretization_step_size="0.050000"
-          mesh_normal_poses="{mesh_normal_poses}"
-          link_name="{mesh_link_name}"
-        />
-        <Action
-          ID="ResetPoseStampedVector"
-          vector="{mesh_normal_poses_reoriented}"
-        />
-        <Decorator
-          ID="ForEach"
-          vector_in="{mesh_normal_poses}"
-          out="{target_pose}"
-          index="{index}"
-        >
+        <Action ID="CreateStampedPose" reference_frame="tool0" stamped_pose="{tool_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;0;0"/>
+        <Action ID="VisualizePose" marker_name="tool_pose" pose="{tool_pose}" marker_lifetime="0.000000" marker_size="0.200000"/>
+        <Action ID="GetMeshNormalPoses" discretization_step_size="0.050000" mesh_normal_poses="{mesh_normal_poses}" link_name="{mesh_link_name}"/>
+        <Action ID="ResetPoseStampedVector" vector="{mesh_normal_poses_reoriented}"/>
+        <Decorator ID="ForEach" vector_in="{mesh_normal_poses}" out="{target_pose}" index="{index}">
           <Control ID="Sequence">
-            <Action
-              ID="TransformPose"
-              input_pose="{target_pose}"
-              output_pose="{output_pose}"
-              quaternion_xyzw="0.7071;0;0.7071;0"
-              translation_xyz="0;0;0"
-            />
-            <Action
-              ID="AddPoseStampedToVector"
-              input="{output_pose}"
-              vector="{mesh_normal_poses_reoriented}"
-            />
+            <Action ID="TransformPose" input_pose="{target_pose}" output_pose="{output_pose}" quaternion_xyzw="0.7071;0;0.7071;0" translation_xyz="0;0;0"/>
+            <Action ID="AddPoseStampedToVector" input="{output_pose}" vector="{mesh_normal_poses_reoriented}"/>
           </Control>
         </Decorator>
       </Control>
-      <Control
-        ID="Sequence"
-        name="Plan to each place pose from an approach pose"
-      >
-        <Action ID="Script" code="pose_count := 0" />
-        <Decorator
-          ID="ForEach"
-          vector_in="{mesh_normal_poses_reoriented}"
-          out="{target_pose}"
-          index="{index}"
-        >
+      <Control ID="Sequence" name="Plan to each place pose from an approach pose">
+        <Action ID="Script" code="pose_count := 0"/>
+        <Decorator ID="ForEach" vector_in="{mesh_normal_poses_reoriented}" out="{target_pose}" index="{index}">
           <Control ID="Sequence">
-            <Action
-              ID="VisualizePose"
-              marker_name="pose"
-              pose="{target_pose}"
-              marker_lifetime="0.000000"
-              marker_size="0.100000"
-            />
-            <Action ID="Script" code="pose_count += 1" />
-            <Action ID="ResetPoseStampedVector" vector="{plan_pose}" />
-            <Action
-              ID="AddPoseStampedToVector"
-              input="{target_pose}"
-              vector="{plan_pose}"
-            />
+            <Action ID="VisualizePose" marker_name="pose" pose="{target_pose}" marker_lifetime="0.000000" marker_size="0.100000"/>
+            <Action ID="Script" code="pose_count += 1"/>
+            <Action ID="ResetPoseStampedVector" vector="{plan_pose}"/>
+            <Action ID="AddPoseStampedToVector" input="{target_pose}" vector="{plan_pose}"/>
             <Control ID="IfThenElse">
               <Control ID="Sequence">
-                <Action
-                  ID="PlanCartesianPath"
-                  trajectory_sampling_rate="30"
-                  debug_solution="{debug_solution}"
-                  velocity_scale_factor="1.000000"
-                  acceleration_scale_factor="1.000000"
-                  blending_radius="0.020000"
-                  position_only="false"
-                  joint_trajectory_msg="{joint_trajectory_msg}"
-                  tip_link="{tip_link}"
-                  planning_group_name="{planning_group_name}"
-                  path="{plan_pose}"
-                  tip_offset="{tip_offset}"
-                  ik_cartesian_space_density="0.010000"
-                  ik_joint_space_density="0.100000"
-                />
-                <Action
-                  ID="WaitForUserTrajectoryApproval"
-                  solution="{debug_solution}"
-                />
+                <Action ID="PlanCartesianPath" trajectory_sampling_rate="30" debug_solution="{debug_solution}" velocity_scale_factor="1.000000" acceleration_scale_factor="1.000000" blending_radius="0.020000" position_only="false" joint_trajectory_msg="{joint_trajectory_msg}" tip_link="{tip_link}" planning_group_name="{planning_group_name}" path="{plan_pose}" tip_offset="{tip_offset}" ik_cartesian_space_density="0.010000" ik_joint_space_density="0.100000"/>
+                <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
               </Control>
-              <Decorator
-                ID="Precondition"
-                else="SUCCESS"
-                if="execute_trajectories == &quot;true&quot;"
-              >
+              <Decorator ID="Precondition" else="SUCCESS" if="execute_trajectories == &quot;true&quot;">
                 <Control ID="Sequence">
-                  <Action
-                    ID="ExecuteFollowJointTrajectory"
-                    execute_follow_joint_trajectory_action_name="{controller_action_server}"
-                    goal_duration_tolerance="-1.000000"
-                    goal_position_tolerance="0.000000"
-                    goal_time_tolerance="0.000000"
-                    joint_trajectory_msg="{joint_trajectory_msg}"
-                  />
-                  <SubTree
-                    ID="Move to Joint State"
-                    _collapsed="true"
-                    target_joint_state="{starting_joint_state}"
-                    acceleration_scale_factor="1.0"
-                    controller_action_server="{controller_action_server}"
-                    controller_names="{controller_name}"
-                    joint_group_name="{planning_group_name}"
-                    keep_orientation="false"
-                    keep_orientation_tolerance="0.05"
-                    link_padding="0.01"
-                    velocity_scale_factor="1.0"
-                  />
+                  <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="{controller_action_server}" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
+                  <SubTree ID="Move to Joint State" _collapsed="true" target_joint_state="{starting_joint_state}" acceleration_scale_factor="1.0" controller_action_server="{controller_action_server}" controller_names="{controller_name}" joint_group_name="{planning_group_name}" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0"/>
                 </Control>
               </Decorator>
-              <Action ID="AlwaysSuccess" />
+              <Action ID="AlwaysSuccess"/>
             </Control>
           </Control>
         </Decorator>
         <!--link6, flange, tool0-->
-        <Action
-          ID="ExecuteFollowJointTrajectory"
-          goal_duration_tolerance="-1.000000"
-          goal_position_tolerance="0.000000"
-          goal_time_tolerance="0.000000"
-          joint_trajectory_msg="{joint_trajectory_msg}"
-          execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-        />
+        <Action ID="ExecuteFollowJointTrajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Reachability Analysis for mesh">
-      <inout_port
-        name="controller_action_server"
-        default="{controller_action_server}"
-      />
-      <inout_port name="controller_name" default="{controller_name}" />
-      <inout_port name="execute_trajectories" default="true" />
-      <inout_port name="mesh_link_name" default="{mesh_link_name}" />
-      <inout_port name="planning_group_name" default="{planning_group_name}" />
-      <inout_port name="starting_waypoint" default="{starting_waypoint}" />
-      <inout_port name="tip_link" default="{tip_link}" />
-      <inout_port name="tip_offset" default="0;0;0;0;0;0" />
+      <inout_port name="controller_action_server" default="{controller_action_server}"/>
+      <inout_port name="controller_name" default="{controller_name}"/>
+      <inout_port name="execute_trajectories" default="true"/>
+      <inout_port name="mesh_link_name" default="{mesh_link_name}"/>
+      <inout_port name="planning_group_name" default="{planning_group_name}"/>
+      <inout_port name="starting_waypoint" default="{starting_waypoint}"/>
+      <inout_port name="tip_link" default="{tip_link}"/>
+      <inout_port name="tip_offset" default="0;0;0;0;0;0"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Reachability" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Reachability"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/reachability_analysis_for_plane.xml b/src/factory_sim/objectives/reachability_analysis_for_plane.xml
index 3adceadd..6c8ce773 100644
--- a/src/factory_sim/objectives/reachability_analysis_for_plane.xml
+++ b/src/factory_sim/objectives/reachability_analysis_for_plane.xml
@@ -1,291 +1,79 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Reachability Analysis for plane">
   <!--//////////-->
-  <BehaviorTree
-    ID="Reachability Analysis for plane"
-    _subtreeOnly="true"
-    column_spacing_translation="0;0.25;0"
-    num_45_deg_rotations="0"
-    num_columns="3"
-    num_rows="3"
-    pick_height_translation="0;0;-0.3"
-    reachability_origin_pose="{reachability_origin_pose}"
-    row_spacing_translation="0.35;0;0"
-    execute_trajectories="true"
-    _description="Given a grid of points, attempt to free space plan to a vertical approach pose, then Cartesian move to the grid point."
-    pick_height="-0.3"
-    starting_waypoint="&quot;Initial Position&quot;"
-    controller_action_server="{controller_action_server}"
-    controller_name="{controller_name}"
-    planning_group_name="{planning_group_name}"
-    tip_link="{tip_link}"
-  >
+  <BehaviorTree ID="Reachability Analysis for plane" _subtreeOnly="true" column_spacing_translation="0;0.25;0" num_45_deg_rotations="0" num_columns="3" num_rows="3" pick_height_translation="0;0;-0.3" reachability_origin_pose="{reachability_origin_pose}" row_spacing_translation="0.35;0;0" execute_trajectories="true" _description="Given a grid of points, attempt to free space plan to a vertical approach pose, then Cartesian move to the grid point." pick_height="-0.3" starting_waypoint="&quot;Initial Position&quot;" controller_action_server="{controller_action_server}" controller_name="{controller_name}" planning_group_name="{planning_group_name}" tip_link="{tip_link}">
     <Control ID="Sequence">
       <Control ID="Sequence" name="Move to starting waypoint">
-        <Action
-          ID="RetrieveWaypoint"
-          joint_group_name="{planning_group_name}"
-          waypoint_joint_state="{starting_joint_state}"
-          waypoint_name="{starting_waypoint}"
-        />
-        <SubTree
-          ID="Move to Joint State"
-          _collapsed="false"
-          target_joint_state="{starting_joint_state}"
-          acceleration_scale_factor="1.0"
-          controller_action_server="{controller_action_server}"
-          controller_names="{controller_name}"
-          joint_group_name="{planning_group_name}"
-          keep_orientation="false"
-          keep_orientation_tolerance="0.05"
-          link_padding="0.01"
-          velocity_scale_factor="1.0"
-        />
+        <Action ID="RetrieveWaypoint" joint_group_name="{planning_group_name}" waypoint_joint_state="{starting_joint_state}" waypoint_name="{starting_waypoint}"/>
+        <SubTree ID="Move to Joint State" _collapsed="false" target_joint_state="{starting_joint_state}" acceleration_scale_factor="1.0" controller_action_server="{controller_action_server}" controller_names="{controller_name}" joint_group_name="{planning_group_name}" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0"/>
       </Control>
       <Control ID="Sequence" name="Visualize key poses">
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="{tip_link}"
-          stamped_pose="{tool_pose}"
-          orientation_xyzw="0;0;0;1"
-          position_xyz="0;0;0"
-        />
-        <Action
-          ID="VisualizePose"
-          marker_name="tool_pose"
-          pose="{tool_pose}"
-          marker_lifetime="0.000000"
-          marker_size="0.200000"
-        />
-        <Action
-          ID="TransformPose"
-          input_pose="{reachability_origin_pose}"
-          output_pose="{starting_pose}"
-          quaternion_xyzw="0.7071;0.7071;0;0.0004"
-          translation_xyz="0;0;0"
-          name="Orient pose to downward wrist frame"
-        />
-        <Action
-          ID="TransformPose"
-          input_pose="{reachability_origin_pose}"
-          output_pose="{starting_pose}"
-          quaternion_xyzw="0;0;0;1"
-          translation_xyz="0;0;0"
-          name="Orient pose to downward wrist frame"
-        />
-        <Action
-          ID="VisualizePose"
-          marker_name="starting_pose"
-          pose="{starting_pose}"
-          marker_lifetime="0.000000"
-          marker_size="0.200000"
-        />
+        <Action ID="CreateStampedPose" reference_frame="{tip_link}" stamped_pose="{tool_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;0;0"/>
+        <Action ID="VisualizePose" marker_name="tool_pose" pose="{tool_pose}" marker_lifetime="0.000000" marker_size="0.200000"/>
+        <Action ID="TransformPose" input_pose="{reachability_origin_pose}" output_pose="{starting_pose}" quaternion_xyzw="0.7071;0.7071;0;0.0004" translation_xyz="0;0;0" name="Orient pose to downward wrist frame"/>
+        <Action ID="TransformPose" input_pose="{reachability_origin_pose}" output_pose="{starting_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;0" name="Orient pose to downward wrist frame"/>
+        <Action ID="VisualizePose" marker_name="starting_pose" pose="{starting_pose}" marker_lifetime="0.000000" marker_size="0.200000"/>
       </Control>
       <Control ID="Sequence" name="Make vector of place poses to plan to">
-        <Action ID="ResetPoseStampedVector" vector="{target_poses}" />
-        <Action
-          ID="TransformPose"
-          input_pose="{starting_pose}"
-          output_pose="{y_incremental_pose}"
-          quaternion_xyzw="0;0;0;1"
-          translation_xyz="0;0;0"
-        />
-        <Action
-          ID="TransformPose"
-          input_pose="{starting_pose}"
-          output_pose="{x_incremental_pose}"
-          quaternion_xyzw="0;0;0;1"
-          translation_xyz="0;0;0"
-        />
+        <Action ID="ResetPoseStampedVector" vector="{target_poses}"/>
+        <Action ID="TransformPose" input_pose="{starting_pose}" output_pose="{y_incremental_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;0"/>
+        <Action ID="TransformPose" input_pose="{starting_pose}" output_pose="{x_incremental_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;0"/>
         <Decorator ID="Repeat" num_cycles="{num_columns}">
           <Control ID="Sequence" name="column/y increments">
             <Decorator ID="Repeat" num_cycles="{num_rows}">
               <Control ID="Sequence" name="row/x increments and angle">
-                <Action
-                  ID="TransformPose"
-                  input_pose="{x_incremental_pose}"
-                  output_pose="{x_incremental_pose_rotated}"
-                  quaternion_xyzw="0;0;0;1"
-                  translation_xyz="0;0;0"
-                />
+                <Action ID="TransformPose" input_pose="{x_incremental_pose}" output_pose="{x_incremental_pose_rotated}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;0"/>
                 <Decorator ID="Repeat" num_cycles="{num_45_deg_rotations}">
-                  <Action
-                    ID="TransformPose"
-                    input_pose="{x_incremental_pose_rotated}"
-                    output_pose="{x_incremental_pose_rotated}"
-                    quaternion_xyzw="0; 0; 0.3826834; 0.9238795"
-                    translation_xyz="0;0;0"
-                  />
+                  <Action ID="TransformPose" input_pose="{x_incremental_pose_rotated}" output_pose="{x_incremental_pose_rotated}" quaternion_xyzw="0; 0; 0.3826834; 0.9238795" translation_xyz="0;0;0"/>
                 </Decorator>
-                <Action
-                  ID="AddPoseStampedToVector"
-                  input="{x_incremental_pose_rotated}"
-                  vector="{target_poses}"
-                />
-                <Action
-                  ID="TransformPose"
-                  input_pose="{x_incremental_pose}"
-                  output_pose="{x_incremental_pose}"
-                  quaternion_xyzw="0;0;0;1"
-                  translation_xyz="{row_spacing_translation}"
-                />
+                <Action ID="AddPoseStampedToVector" input="{x_incremental_pose_rotated}" vector="{target_poses}"/>
+                <Action ID="TransformPose" input_pose="{x_incremental_pose}" output_pose="{x_incremental_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="{row_spacing_translation}"/>
               </Control>
             </Decorator>
-            <Action
-              ID="TransformPose"
-              input_pose="{y_incremental_pose}"
-              output_pose="{y_incremental_pose}"
-              quaternion_xyzw="0;0;0;1"
-              translation_xyz="{column_spacing_translation}"
-            />
-            <Action
-              ID="TransformPose"
-              input_pose="{y_incremental_pose}"
-              output_pose="{x_incremental_pose}"
-              quaternion_xyzw="0;0;0;1"
-              translation_xyz="0;0;0"
-            />
+            <Action ID="TransformPose" input_pose="{y_incremental_pose}" output_pose="{y_incremental_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="{column_spacing_translation}"/>
+            <Action ID="TransformPose" input_pose="{y_incremental_pose}" output_pose="{x_incremental_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;0"/>
           </Control>
         </Decorator>
       </Control>
       <Control ID="Sequence" name="Visualize vector of place poses">
         <!--Use a script with a pose counter so we can give a unique name to each marker (allowing multiple poses to be visualized)-->
-        <Action ID="Script" code="pose_count := 0" />
-        <Decorator
-          ID="ForEach"
-          vector_in="{target_poses}"
-          out="{target_pose}"
-          index="{index}"
-        >
+        <Action ID="Script" code="pose_count := 0"/>
+        <Decorator ID="ForEach" vector_in="{target_poses}" out="{target_pose}" index="{index}">
           <Control ID="Sequence">
             <!--Visualize and add each pose to the vector-->
-            <Action
-              ID="VisualizePose"
-              marker_name="{pose_count}"
-              pose="{target_pose}"
-              marker_lifetime="0.000000"
-              marker_size="0.100000"
-            />
-            <Action ID="Script" code="pose_count += 1" />
+            <Action ID="VisualizePose" marker_name="{pose_count}" pose="{target_pose}" marker_lifetime="0.000000" marker_size="0.100000"/>
+            <Action ID="Script" code="pose_count += 1"/>
           </Control>
         </Decorator>
       </Control>
-      <Control
-        ID="Sequence"
-        name="Plan to each place pose from an approach pose"
-      >
-        <Decorator
-          ID="ForEach"
-          vector_in="{target_poses}"
-          index="{index}"
-          out="{place_pose}"
-        >
+      <Control ID="Sequence" name="Plan to each place pose from an approach pose">
+        <Decorator ID="ForEach" vector_in="{target_poses}" index="{index}" out="{place_pose}">
           <Control ID="Sequence">
             <Control ID="Sequence" name="Make approach pose">
-              <Action
-                ID="TransformPose"
-                output_pose="{place_approach_pose}"
-                input_pose="{place_pose}"
-                translation_xyz="{pick_height_translation}"
-                quaternion_xyzw="0;0;0;1"
-              />
-              <Action
-                ID="VisualizePose"
-                marker_name="place_approach_pose"
-                marker_lifetime="0.000000"
-                marker_size="0.100000"
-                pose="{place_approach_pose}"
-              />
+              <Action ID="TransformPose" output_pose="{place_approach_pose}" input_pose="{place_pose}" translation_xyz="{pick_height_translation}" quaternion_xyzw="0;0;0;1"/>
+              <Action ID="VisualizePose" marker_name="place_approach_pose" marker_lifetime="0.000000" marker_size="0.100000" pose="{place_approach_pose}"/>
             </Control>
             <Control ID="Sequence" name="Plan to approach then place">
-              <Action
-                ID="ActivateControllers"
-                controller_names="{controller_name}"
-              />
-              <Action
-                ID="InitializeMTCTask"
-                controller_names="{controller_name}"
-                task="{mtc_task}"
-                task_id="move_to_pose"
-              />
-              <Action
-                ID="SetupMTCCurrentState"
-                skip_collision_check="true"
-                task="{mtc_task}"
-              />
-              <Action
-                ID="SetupMTCPlanToPose"
-                acceleration_scale_factor="1.000000"
-                ik_frame="{tip_link}"
-                keep_orientation="false"
-                link_padding="0.000000"
-                max_iterations="50000"
-                monitored_stage="current state"
-                planning_group_name="{planning_group_name}"
-                target_pose="{place_approach_pose}"
-                trajectory_sampling_rate="100"
-                velocity_scale_factor="1.000000"
-                task="{mtc_task}"
-                keep_orientation_tolerance="3"
-                keep_orientation_link_names="grasp_link"
-              />
-              <Action
-                ID="SetupMTCMoveAlongFrameAxis"
-                acceleration_scale=".5"
-                axis_frame="{tip_link}"
-                axis_x="0.000000"
-                axis_y="0.000000"
-                axis_z="1"
-                hand_frame="{tip_link}"
-                ignore_environment_collisions="true"
-                max_distance="{pick_height}"
-                min_distance="{pick_height}"
-                planning_group_name="{planning_group_name}"
-                velocity_scale=".5"
-                task="{mtc_task}"
-              />
+              <Action ID="ActivateControllers" controller_names="{controller_name}"/>
+              <Action ID="InitializeMTCTask" controller_names="{controller_name}" task="{mtc_task}" task_id="move_to_pose"/>
+              <Action ID="SetupMTCCurrentState" skip_collision_check="true" task="{mtc_task}"/>
+              <Action ID="SetupMTCPlanToPose" acceleration_scale_factor="1.000000" ik_frame="{tip_link}" keep_orientation="false" link_padding="0.000000" max_iterations="50000" monitored_stage="current state" planning_group_name="{planning_group_name}" target_pose="{place_approach_pose}" trajectory_sampling_rate="100" velocity_scale_factor="1.000000" task="{mtc_task}" keep_orientation_tolerance="3" keep_orientation_link_names="grasp_link"/>
+              <Action ID="SetupMTCMoveAlongFrameAxis" acceleration_scale=".5" axis_frame="{tip_link}" axis_x="0.000000" axis_y="0.000000" axis_z="1" hand_frame="{tip_link}" ignore_environment_collisions="true" max_distance="{pick_height}" min_distance="{pick_height}" planning_group_name="{planning_group_name}" velocity_scale=".5" task="{mtc_task}"/>
               <Decorator ID="ForceSuccess">
                 <Control ID="IfThenElse">
                   <Control ID="Sequence">
-                    <Action
-                      ID="PlanMTCTask"
-                      task="{mtc_task}"
-                      solution="{mtc_solution}"
-                    />
-                    <Action
-                      ID="WaitForUserTrajectoryApproval"
-                      solution="{mtc_solution}"
-                    />
+                    <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}"/>
+                    <Action ID="WaitForUserTrajectoryApproval" solution="{mtc_solution}"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Decorator
-                      ID="Precondition"
-                      else="SUCCESS"
-                      if="execute_trajectories == &quot;true&quot;"
-                    >
+                    <Decorator ID="Precondition" else="SUCCESS" if="execute_trajectories == &quot;true&quot;">
                       <Control ID="Sequence">
-                        <Action
-                          ID="ExecuteMTCTask"
-                          goal_duration_tolerance="-1.000000"
-                          solution="{mtc_solution}"
-                        />
-                        <SubTree
-                          ID="Move to Joint State"
-                          _collapsed="true"
-                          target_joint_state="{starting_joint_state}"
-                          acceleration_scale_factor="1.0"
-                          controller_action_server="{controller_action_server}"
-                          controller_names="{controller_name}"
-                          joint_group_name="{planning_group_name}"
-                          keep_orientation="false"
-                          keep_orientation_tolerance="0.05"
-                          link_padding="0.01"
-                          velocity_scale_factor="1.0"
-                        />
+                        <Action ID="ExecuteMTCTask" goal_duration_tolerance="-1.000000" solution="{mtc_solution}"/>
+                        <SubTree ID="Move to Joint State" _collapsed="true" target_joint_state="{starting_joint_state}" acceleration_scale_factor="1.0" controller_action_server="{controller_action_server}" controller_names="{controller_name}" joint_group_name="{planning_group_name}" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0"/>
                       </Control>
                     </Decorator>
                   </Control>
-                  <Action ID="AlwaysSuccess" />
+                  <Action ID="AlwaysSuccess"/>
                 </Control>
               </Decorator>
             </Control>
@@ -296,32 +84,23 @@
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Reachability Analysis for plane">
-      <inout_port name="column_spacing_translation" default="0;0.25;0" />
-      <inout_port
-        name="controller_action_server"
-        default="{controller_action_server}"
-      />
-      <inout_port name="controller_name" default="{controller_name}" />
-      <inout_port name="execute_trajectories" default="true" />
-      <inout_port name="num_45_deg_rotations" default="0" />
-      <inout_port name="num_columns" default="3" />
-      <inout_port name="num_rows" default="3" />
-      <inout_port name="pick_height" default="-0.3" />
-      <inout_port name="pick_height_translation" default="0;0;-0.3" />
-      <inout_port name="planning_group_name" default="{planning_group_name}" />
-      <inout_port
-        name="reachability_origin_pose"
-        default="{reachability_origin_pose}"
-      />
-      <inout_port name="row_spacing_translation" default="0.35;0;0" />
-      <inout_port
-        name="starting_waypoint"
-        default="&quot;Initial Position&quot;"
-      />
-      <inout_port name="tip_link" default="{tip_link}" />
+      <inout_port name="column_spacing_translation" default="0;0.25;0"/>
+      <inout_port name="controller_action_server" default="{controller_action_server}"/>
+      <inout_port name="controller_name" default="{controller_name}"/>
+      <inout_port name="execute_trajectories" default="true"/>
+      <inout_port name="num_45_deg_rotations" default="0"/>
+      <inout_port name="num_columns" default="3"/>
+      <inout_port name="num_rows" default="3"/>
+      <inout_port name="pick_height" default="-0.3"/>
+      <inout_port name="pick_height_translation" default="0;0;-0.3"/>
+      <inout_port name="planning_group_name" default="{planning_group_name}"/>
+      <inout_port name="reachability_origin_pose" default="{reachability_origin_pose}"/>
+      <inout_port name="row_spacing_translation" default="0.35;0;0"/>
+      <inout_port name="starting_waypoint" default="&quot;Initial Position&quot;"/>
+      <inout_port name="tip_link" default="{tip_link}"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Reachability" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Reachability"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/request_teleoperation.xml b/src/factory_sim/objectives/request_teleoperation.xml
index 2b7f3402..f8e448cf 100644
--- a/src/factory_sim/objectives/request_teleoperation.xml
+++ b/src/factory_sim/objectives/request_teleoperation.xml
@@ -1,64 +1,32 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Request Teleoperation">
   <!--The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message.-->
-  <BehaviorTree
-    ID="Request Teleoperation"
-    _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective."
-    _hardcoded="false"
-    _subtreeOnly="true"
-    enable_user_interaction="false"
-    initial_teleop_mode="3"
-    user_interaction_prompt="Teleoperate"
-  >
+  <BehaviorTree ID="Request Teleoperation" _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective." _hardcoded="false" _subtreeOnly="true" enable_user_interaction="false" initial_teleop_mode="3" user_interaction_prompt="Teleoperate">
     <Control ID="Sequence">
-      <Action ID="Script" code="teleop_mode := 0" />
+      <Action ID="Script" code="teleop_mode := 0"/>
       <Control ID="Parallel" success_count="1" failure_count="1">
-        <Action
-          ID="DoTeleoperateAction"
-          enable_user_interaction="{enable_user_interaction}"
-          user_interaction_prompt="{user_interaction_prompt}"
-          initial_teleop_mode="{initial_teleop_mode}"
-          current_teleop_mode="{teleop_mode}"
-        />
+        <Action ID="DoTeleoperateAction" enable_user_interaction="{enable_user_interaction}" user_interaction_prompt="{user_interaction_prompt}" initial_teleop_mode="{initial_teleop_mode}" current_teleop_mode="{teleop_mode}"/>
         <Decorator ID="KeepRunningUntilFailure">
           <Control ID="Sequence">
             <!--Closing and opening the gripper-->
             <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 7">
-              <SubTree ID="Activate Vacuum" />
+              <SubTree ID="Activate Vacuum"/>
             </Decorator>
             <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 6">
-              <SubTree ID="Deactivate Vacuum" />
+              <SubTree ID="Deactivate Vacuum"/>
             </Decorator>
             <!--Joint sliders interpolate to joint state-->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 5">
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrieveJointStateParameter"
-                      timeout_sec="-1"
-                      joint_state="{target_joint_state}"
-                    />
-                    <SubTree
-                      ID="Interpolate to Joint State"
-                      _collapsed="false"
-                      target_joint_state="{target_joint_state}"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
+                    <SubTree ID="Interpolate to Joint State" _collapsed="false" target_joint_state="{target_joint_state}"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
@@ -68,31 +36,13 @@
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrievePoseParameter"
-                      timeout_sec="-1"
-                      pose="{target_pose}"
-                    />
-                    <SubTree
-                      ID="Move to Pose"
-                      _collapsed="false"
-                      target_pose="{target_pose}"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrievePoseParameter" timeout_sec="-1" pose="{target_pose}"/>
+                    <SubTree ID="Move to Pose" _collapsed="false" target_pose="{target_pose}"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
@@ -102,55 +52,23 @@
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrieveJointStateParameter"
-                      timeout_sec="-1"
-                      joint_state="{target_joint_state}"
-                    />
-                    <SubTree
-                      ID="Move to Joint State"
-                      _collapsed="false"
-                      target_joint_state="{target_joint_state}"
-                      acceleration_scale_factor="1.0"
-                      controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-                      controller_names="joint_trajectory_controller"
-                      joint_group_name="manipulator"
-                      keep_orientation="false"
-                      keep_orientation_tolerance="0.05"
-                      link_padding="0.01"
-                      velocity_scale_factor="1.0"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
+                    <SubTree ID="Move to Joint State" _collapsed="false" target_joint_state="{target_joint_state}" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
             </Decorator>
             <!--Cartesian and joint jogging-->
             <Control ID="Sequence" _while="teleop_mode == 2">
-              <Action
-                ID="TeleoperateTwist"
-                controller_name="servo_controller"
-              />
+              <Action ID="TeleoperateTwist" controller_name="servo_controller"/>
             </Control>
             <Control ID="Sequence" _while="teleop_mode == 1">
-              <Action
-                ID="TeleoperateJointJog"
-                controller_name="servo_controller"
-              />
+              <Action ID="TeleoperateJointJog" controller_name="servo_controller"/>
             </Control>
           </Control>
         </Decorator>
@@ -159,12 +77,12 @@
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Request Teleoperation">
-      <input_port name="enable_user_interaction" default="false" />
-      <input_port name="initial_teleop_mode" default="3" />
-      <input_port name="user_interaction_prompt" default="Teleoperate" />
+      <input_port name="enable_user_interaction" default="false"/>
+      <input_port name="initial_teleop_mode" default="3"/>
+      <input_port name="user_interaction_prompt" default="Teleoperate"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="User Input" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="User Input"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/reset_planning_scene.xml b/src/factory_sim/objectives/reset_planning_scene.xml
index 8da31539..ba9a1d4e 100644
--- a/src/factory_sim/objectives/reset_planning_scene.xml
+++ b/src/factory_sim/objectives/reset_planning_scene.xml
@@ -1,20 +1,17 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Reset Planning Scene">
   <!--//////////-->
   <BehaviorTree ID="Reset Planning Scene" _description="" _hardcoded="false">
-    <Action
-      ID="ResetPlanningSceneObjects"
-      apply_planning_scene_service="/apply_planning_scene"
-    />
+    <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Reset Planning Scene">
       <MetadataFields>
-        <Metadata subcategory="Motion - Planning" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Motion - Planning"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
       <MetadataFields>
-        <Metadata subcategory="Motion - Planning" />
+        <Metadata subcategory="Motion - Planning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/retract.xml b/src/factory_sim/objectives/retract.xml
index 47e3a021..0f3a1377 100644
--- a/src/factory_sim/objectives/retract.xml
+++ b/src/factory_sim/objectives/retract.xml
@@ -1,53 +1,21 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Retract">
   <!--//////////-->
-  <BehaviorTree
-    ID="Retract"
-    _description="Move back a distance in the tool frame"
-    _subtreeOnly="true"
-    distance="0.1"
-  >
+  <BehaviorTree ID="Retract" _description="Move back a distance in the tool frame" _subtreeOnly="true" distance="0.1">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="InitializeMTCTask"
-        controller_names="/joint_trajectory_controller"
-        task_id="retract"
-        task="{mtc_task}"
-      />
-      <Action
-        ID="SetupMTCCurrentState"
-        skip_collision_check="true"
-        task="{mtc_task}"
-      />
-      <Action
-        ID="SetupMTCMoveAlongFrameAxis"
-        axis_frame="tool0"
-        axis_z="-1"
-        max_distance="{distance}"
-        min_distance="{distance}"
-        hand_frame="tool0"
-        acceleration_scale="0.5"
-        axis_x="0.000000"
-        axis_y="0.000000"
-        ignore_environment_collisions="false"
-        planning_group_name="manipulator"
-        velocity_scale="0.5"
-        task="{mtc_task}"
-      />
-      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
-      <Action
-        ID="ExecuteMTCTask"
-        goal_duration_tolerance="-1.000000"
-        solution="{mtc_solution}"
-      />
+      <Action ID="InitializeMTCTask" controller_names="/joint_trajectory_controller" task_id="retract" task="{mtc_task}"/>
+      <Action ID="SetupMTCCurrentState" skip_collision_check="true" task="{mtc_task}"/>
+      <Action ID="SetupMTCMoveAlongFrameAxis" axis_frame="tool0" axis_z="-1" max_distance="{distance}" min_distance="{distance}" hand_frame="tool0" acceleration_scale="0.5" axis_x="0.000000" axis_y="0.000000" ignore_environment_collisions="false" planning_group_name="manipulator" velocity_scale="0.5" task="{mtc_task}"/>
+      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}"/>
+      <Action ID="ExecuteMTCTask" goal_duration_tolerance="-1.000000" solution="{mtc_solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Retract">
-      <input_port name="distance" default="0.1" />
+      <input_port name="distance" default="0.1"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/setup_starting_planning_scene.xml b/src/factory_sim/objectives/setup_starting_planning_scene.xml
index 48feb3f4..8959a737 100644
--- a/src/factory_sim/objectives/setup_starting_planning_scene.xml
+++ b/src/factory_sim/objectives/setup_starting_planning_scene.xml
@@ -1,29 +1,16 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Setup Initial Planning Scene">
   <!--//////////-->
-  <BehaviorTree
-    ID="Setup Initial Planning Scene"
-    _description="Sets the tool at its initial pose in the Planning Scene"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Setup Initial Planning Scene" _description="Sets the tool at its initial pose in the Planning Scene" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        stamped_pose="{tool_pose}"
-        reference_frame="tool_attach_site"
-      />
-      <Action
-        ID="AddToolToScene"
-        relative_pose="{tool_pose}"
-        package_name="factory_sim"
-        tool_urdf_file_path="description/tool.urdf"
-        tool_name="suction_gripper"
-      />
+      <Action ID="CreateStampedPose" stamped_pose="{tool_pose}" reference_frame="tool_attach_site"/>
+      <Action ID="AddToolToScene" relative_pose="{tool_pose}" package_name="factory_sim" tool_urdf_file_path="description/tool.urdf" tool_name="suction_gripper"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Setup Initial Planning Scene">
       <MetadataFields>
-        <Metadata subcategory="Motion - Planning" />
+        <Metadata subcategory="Motion - Planning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/take_snapshot.xml b/src/factory_sim/objectives/take_snapshot.xml
index 06415132..c1c4fba2 100644
--- a/src/factory_sim/objectives/take_snapshot.xml
+++ b/src/factory_sim/objectives/take_snapshot.xml
@@ -1,30 +1,17 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <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"
-  >
+  <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="/scene_camera/points"
-        message_out="{point_cloud}"
-        timeout_sec="15.000000"
-      />
-      <Action
-        ID="SendPointCloudToUI"
-        point_cloud="{point_cloud}"
-        pcd_topic="/pcd_pointcloud_captures"
-      />
+      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
+      <Action ID="GetPointCloud" topic_name="/scene_camera/points" message_out="{point_cloud}" timeout_sec="15.000000"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Take Snapshot">
       <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
+        <Metadata subcategory="Perception - 3D Point Cloud"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/teleoperate.xml b/src/factory_sim/objectives/teleoperate.xml
index e9343282..20da5e31 100644
--- a/src/factory_sim/objectives/teleoperate.xml
+++ b/src/factory_sim/objectives/teleoperate.xml
@@ -1,23 +1,14 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Teleoperate">
   <!--//////////-->
-  <BehaviorTree
-    ID="Teleoperate"
-    _description="Handles the different variations of teleoperation from the web UI. Can be used standalone."
-    _hardcoded="false"
-  >
-    <SubTree
-      ID="Request Teleoperation"
-      enable_user_interaction="false"
-      user_interaction_prompt="Teleoperate"
-      initial_teleop_mode="3"
-    />
+  <BehaviorTree ID="Teleoperate" _description="Handles the different variations of teleoperation from the web UI. Can be used standalone." _hardcoded="false">
+    <SubTree ID="Request Teleoperation" enable_user_interaction="false" user_interaction_prompt="Teleoperate" initial_teleop_mode="3"/>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Teleoperate">
       <MetadataFields>
-        <Metadata subcategory="User Input" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="User Input"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/tool_attachment_example.xml b/src/factory_sim/objectives/tool_attachment_example.xml
index af27dec4..a3becd7e 100644
--- a/src/factory_sim/objectives/tool_attachment_example.xml
+++ b/src/factory_sim/objectives/tool_attachment_example.xml
@@ -1,23 +1,19 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Tool Attachment Example">
   <!--//////////-->
-  <BehaviorTree
-    ID="Tool Attachment Example"
-    _description="Pick up a vacuum gripper, pick up an object with it, drop the object and put the tool back in its holder"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Tool Attachment Example" _description="Pick up a vacuum gripper, pick up an object with it, drop the object and put the tool back in its holder" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Pick Up Tool" _collapsed="true" />
-      <Action ID="WaitForDuration" delay_duration="2" />
-      <SubTree ID="Deactivate Vacuum" _collapsed="true" />
-      <SubTree ID="Place Tool in Tool Holder" _collapsed="true" />
+      <SubTree ID="Pick Up Tool" _collapsed="true"/>
+      <Action ID="WaitForDuration" delay_duration="2"/>
+      <SubTree ID="Deactivate Vacuum" _collapsed="true"/>
+      <SubTree ID="Place Tool in Tool Holder" _collapsed="true"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Tool Attachment Example">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/objectives/wait_for_trajectory_approval_if_user_available.xml b/src/factory_sim/objectives/wait_for_trajectory_approval_if_user_available.xml
index cab88a3e..e529b4b7 100644
--- a/src/factory_sim/objectives/wait_for_trajectory_approval_if_user_available.xml
+++ b/src/factory_sim/objectives/wait_for_trajectory_approval_if_user_available.xml
@@ -1,25 +1,18 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Wait for Trajectory Approval if User Available"
->
-  <BehaviorTree
-    ID="Wait for Trajectory Approval if User Available"
-    _description="Preview a shared pointer to an MTC Solution object via an input data port if a user is available."
-    _subtreeOnly="true"
-  >
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Wait for Trajectory Approval if User Available">
+  <BehaviorTree ID="Wait for Trajectory Approval if User Available" _description="Preview a shared pointer to an MTC Solution object via an input data port if a user is available." _subtreeOnly="true">
     <Control ID="Fallback" name="wait_for_approval_if_user_available">
       <Decorator ID="Inverter">
-        <Action ID="IsUserAvailable" />
+        <Action ID="IsUserAvailable"/>
       </Decorator>
-      <Action ID="WaitForUserTrajectoryApproval" solution="{solution}" />
+      <Action ID="WaitForUserTrajectoryApproval" solution="{solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Wait for Trajectory Approval if User Available">
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="User Input" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="User Input"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/factory_sim/package.xml b/src/factory_sim/package.xml
index a3b23e09..8d4f159a 100644
--- a/src/factory_sim/package.xml
+++ b/src/factory_sim/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>factory_sim</name>
   <version>7.2.0</version>
diff --git a/src/grinding_sim/description/scene.xml b/src/grinding_sim/description/scene.xml
index ee3da58f..df757458 100644
--- a/src/grinding_sim/description/scene.xml
+++ b/src/grinding_sim/description/scene.xml
@@ -1,349 +1,184 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="ur20 grinding scene">
-  <compiler angle="radian" autolimits="true" assetdir="grinding_sim_assets" />
+  <compiler angle="radian" autolimits="true" assetdir="grinding_sim_assets"/>
 
   <default>
     <!-- Actuator & joint defaults -->
-    <joint axis="0 0 1" range="-6.28319 6.28319" armature="0.5" />
-    <position ctrlrange="-6.2831 6.2831" />
+    <joint axis="0 0 1" range="-6.28319 6.28319" armature="0.5"/>
+    <position ctrlrange="-6.2831 6.2831"/>
     <default class="high_gain">
-      <position kp="500000" kv="500" />
+      <position kp="500000" kv="500"/>
     </default>
     <default class="size4">
-      <joint damping="2000" />
+      <joint damping="2000"/>
     </default>
     <default class="size3">
-      <joint damping="150" />
+      <joint damping="150"/>
       <default class="size3_limited">
-        <joint range="-3.1415 3.1415" />
+        <joint range="-3.1415 3.1415"/>
       </default>
     </default>
     <default class="size2">
-      <joint damping="20" />
+      <joint damping="20"/>
     </default>
     <!-- Geom defaults -->
     <default class="visual">
-      <geom group="2" type="mesh" contype="0" conaffinity="0" />
+      <geom group="2" type="mesh" contype="0" conaffinity="0"/>
     </default>
     <default class="collision">
-      <geom group="3" type="mesh" />
+      <geom group="3" type="mesh"/>
     </default>
   </default>
 
   <asset>
     <!-- Textures and materials -->
-    <material name="metal" rgba="0.58 0.58 0.58 1" />
-    <texture type="2d" name="UR20_DIFF_8bit_2K" file="UR20_DIFF_8bit_2K.png" />
-    <material
-      name="UR20_DIFF_8bit_2K_png"
-      texture="UR20_DIFF_8bit_2K"
-      specular="0.0"
-      shininess="0.25"
-    />
+    <material name="metal" rgba="0.58 0.58 0.58 1"/>
+    <texture type="2d" name="UR20_DIFF_8bit_2K" file="UR20_DIFF_8bit_2K.png"/>
+    <material name="UR20_DIFF_8bit_2K_png" texture="UR20_DIFF_8bit_2K" specular="0.0" shininess="0.25"/>
     <!-- ur20 meshes -->
-    <mesh file="ur20/base/base.obj" />
-    <mesh file="ur20/shoulder/shoulder.obj" />
-    <mesh file="ur20/upperarm/upperarm_0.obj" />
-    <mesh file="ur20/upperarm/upperarm_1.obj" />
-    <mesh file="ur20/forearm/forearm.obj" />
-    <mesh file="ur20/wrist1/wrist1.obj" />
-    <mesh file="ur20/wrist2/wrist2.obj" />
-    <mesh file="ur20/wrist3/wrist3.obj" />
+    <mesh file="ur20/base/base.obj"/>
+    <mesh file="ur20/shoulder/shoulder.obj"/>
+    <mesh file="ur20/upperarm/upperarm_0.obj"/>
+    <mesh file="ur20/upperarm/upperarm_1.obj"/>
+    <mesh file="ur20/forearm/forearm.obj"/>
+    <mesh file="ur20/wrist1/wrist1.obj"/>
+    <mesh file="ur20/wrist2/wrist2.obj"/>
+    <mesh file="ur20/wrist3/wrist3.obj"/>
     <!-- shop mesh -->
-    <mesh name="workcell" file="workcell.obj" />
+    <mesh name="workcell" file="workcell.obj"/>
 
     <!-- Tool-Base geometries -->
-    <mesh file="tool_base/tool_base.obj" />
-    <mesh name="base_geometry_0" file="tool_base/geometry_0.stl" />
-    <mesh name="base_geometry_1" file="tool_base/geometry_1.stl" />
-    <mesh name="base_geometry_2" file="tool_base/geometry_2.stl" />
-    <mesh name="base_geometry_3" file="tool_base/geometry_3.stl" />
-    <mesh name="base_geometry_4" file="tool_base/geometry_4.stl" />
-    <mesh name="base_geometry_5" file="tool_base/geometry_5.stl" />
-    <mesh name="base_geometry_6" file="tool_base/geometry_6.stl" />
-    <mesh name="base_geometry_7" file="tool_base/geometry_7.stl" />
+    <mesh file="tool_base/tool_base.obj"/>
+    <mesh name="base_geometry_0" file="tool_base/geometry_0.stl"/>
+    <mesh name="base_geometry_1" file="tool_base/geometry_1.stl"/>
+    <mesh name="base_geometry_2" file="tool_base/geometry_2.stl"/>
+    <mesh name="base_geometry_3" file="tool_base/geometry_3.stl"/>
+    <mesh name="base_geometry_4" file="tool_base/geometry_4.stl"/>
+    <mesh name="base_geometry_5" file="tool_base/geometry_5.stl"/>
+    <mesh name="base_geometry_6" file="tool_base/geometry_6.stl"/>
+    <mesh name="base_geometry_7" file="tool_base/geometry_7.stl"/>
 
     <!-- Tool-Tool geometries -->
-    <mesh file="tool0_tool/tool0_tool.obj" />
-    <mesh name="tool0_tool_1" file="tool0_tool/tool0_tool_1.obj" />
-    <mesh name="tool0_tool_2" file="tool0_tool/tool0_tool_2.obj" />
-    <mesh name="tool0_tool_3" file="tool0_tool/tool0_tool_3.obj" />
-    <mesh name="tool0_tool_4" file="tool0_tool/tool0_tool_4.obj" />
-    <mesh name="tool_geometry_0" file="tool0_tool/geometry_0.stl" />
-    <mesh name="tool_geometry_1" file="tool0_tool/geometry_1.stl" />
-    <mesh name="tool_geometry_2" file="tool0_tool/geometry_2.stl" />
-    <mesh name="tool_geometry_3" file="tool0_tool/geometry_3.stl" />
-    <mesh name="tool_geometry_4" file="tool0_tool/geometry_4.stl" />
-    <mesh name="tool_geometry_5" file="tool0_tool/geometry_5.stl" />
-    <mesh name="tool_geometry_6" file="tool0_tool/geometry_6.stl" />
+    <mesh file="tool0_tool/tool0_tool.obj"/>
+    <mesh name="tool0_tool_1" file="tool0_tool/tool0_tool_1.obj"/>
+    <mesh name="tool0_tool_2" file="tool0_tool/tool0_tool_2.obj"/>
+    <mesh name="tool0_tool_3" file="tool0_tool/tool0_tool_3.obj"/>
+    <mesh name="tool0_tool_4" file="tool0_tool/tool0_tool_4.obj"/>
+    <mesh name="tool_geometry_0" file="tool0_tool/geometry_0.stl"/>
+    <mesh name="tool_geometry_1" file="tool0_tool/geometry_1.stl"/>
+    <mesh name="tool_geometry_2" file="tool0_tool/geometry_2.stl"/>
+    <mesh name="tool_geometry_3" file="tool0_tool/geometry_3.stl"/>
+    <mesh name="tool_geometry_4" file="tool0_tool/geometry_4.stl"/>
+    <mesh name="tool_geometry_5" file="tool0_tool/geometry_5.stl"/>
+    <mesh name="tool_geometry_6" file="tool0_tool/geometry_6.stl"/>
 
     <!-- Engine block part geometries -->
-    <mesh file="engine_block/engine_block.obj" />
-    <mesh name="engine_block_geometry_0" file="engine_block/geometry_0.stl" />
-    <mesh name="engine_block_geometry_1" file="engine_block/geometry_1.stl" />
-    <mesh name="engine_block_geometry_2" file="engine_block/geometry_2.stl" />
-    <mesh name="engine_block_geometry_3" file="engine_block/geometry_3.stl" />
-    <mesh name="engine_block_geometry_4" file="engine_block/geometry_4.stl" />
-    <mesh name="engine_block_geometry_5" file="engine_block/geometry_5.stl" />
-    <mesh name="engine_block_geometry_6" file="engine_block/geometry_6.stl" />
+    <mesh file="engine_block/engine_block.obj"/>
+    <mesh name="engine_block_geometry_0" file="engine_block/geometry_0.stl"/>
+    <mesh name="engine_block_geometry_1" file="engine_block/geometry_1.stl"/>
+    <mesh name="engine_block_geometry_2" file="engine_block/geometry_2.stl"/>
+    <mesh name="engine_block_geometry_3" file="engine_block/geometry_3.stl"/>
+    <mesh name="engine_block_geometry_4" file="engine_block/geometry_4.stl"/>
+    <mesh name="engine_block_geometry_5" file="engine_block/geometry_5.stl"/>
+    <mesh name="engine_block_geometry_6" file="engine_block/geometry_6.stl"/>
   </asset>
 
-  <statistic center="0.3 0 0.4" extent="0.8" />
+  <statistic center="0.3 0 0.4" extent="0.8"/>
 
   <visual>
-    <headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0 0 0" />
-    <rgba haze="0.15 0.25 0.35 1" />
-    <global azimuth="120" elevation="-20" />
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0 0 0"/>
+    <rgba haze="0.15 0.25 0.35 1"/>
+    <global azimuth="120" elevation="-20"/>
   </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"/>
     <!-- Define the workcell with the engine block part -->
     <body name="workcell" pos="0 0 0">
-      <geom material="metal" mesh="workcell" class="visual" />
+      <geom material="metal" mesh="workcell" class="visual"/>
       <!-- Floor -->
       <!--<geom type="box" pos="0 -0.7 -0.05" size="1.385 1.4 0.01" class="collision"/>-->
-      <geom type="box" pos="0 -0.7 -0.05" size="1 1 0.01" class="collision" />
+      <geom type="box" pos="0 -0.7 -0.05" size="1 1 0.01" class="collision"/>
       <!-- Table -->
-      <geom
-        type="box"
-        pos="0 -1.69 0.35"
-        size="0.55 0.34 0.370149"
-        class="collision"
-      />
+      <geom type="box" pos="0 -1.69 0.35" size="0.55 0.34 0.370149" class="collision"/>
       <body name="engine_block" pos="0 -1.69 0.71" quat="0.707 0 0 0.707">
-        <site name="engine_block_pose" />
-        <geom material="metal" mesh="engine_block" class="visual" />
-        <geom mesh="engine_block_geometry_0" class="collision" />
-        <geom mesh="engine_block_geometry_1" class="collision" />
-        <geom mesh="engine_block_geometry_2" class="collision" />
-        <geom mesh="engine_block_geometry_3" class="collision" />
-        <geom mesh="engine_block_geometry_4" class="collision" />
-        <geom mesh="engine_block_geometry_5" class="collision" />
-        <geom mesh="engine_block_geometry_6" class="collision" />
+        <site name="engine_block_pose"/>
+        <geom material="metal" mesh="engine_block" class="visual"/>
+        <geom mesh="engine_block_geometry_0" class="collision"/>
+        <geom mesh="engine_block_geometry_1" class="collision"/>
+        <geom mesh="engine_block_geometry_2" class="collision"/>
+        <geom mesh="engine_block_geometry_3" class="collision"/>
+        <geom mesh="engine_block_geometry_4" class="collision"/>
+        <geom mesh="engine_block_geometry_5" class="collision"/>
+        <geom mesh="engine_block_geometry_6" class="collision"/>
       </body>
     </body>
     <!-- Add a scene camera -->
-    <site
-      name="scene_camera_optical_frame"
-      pos="0.0 -2.56685 1.62333"
-      quat="-0.399763 0.916616 -0.001843 0.000804"
-    />
-    <camera
-      name="scene_camera"
-      pos="0.0 -2.56685 1.62333"
-      fovy="58"
-      mode="fixed"
-      resolution="640 480"
-      quat="0.916616 0.399763 -0.000804 -0.001843"
-    />
+    <site name="scene_camera_optical_frame" pos="0.0 -2.56685 1.62333" quat="-0.399763 0.916616 -0.001843 0.000804"/>
+    <camera name="scene_camera" pos="0.0 -2.56685 1.62333" fovy="58" mode="fixed" resolution="640 480" quat="0.916616 0.399763 -0.000804 -0.001843"/>
 
     <!-- Define the robot -->
     <body name="base_link" pos="0 0 0" quat="-1 0 0 0">
-      <inertial
-        pos="0 0 0"
-        quat="0 0 0 1"
-        mass="4.0"
-        diaginertia="0.0061063308908 0.0061063308908 0.01125"
-      />
-      <geom
-        quat="-1 0 0 0"
-        material="UR20_DIFF_8bit_2K_png"
-        mesh="base"
-        class="visual"
-      />
-      <geom quat="-1 0 0 0" mesh="base" class="collision" />
+      <inertial pos="0 0 0" quat="0 0 0 1" mass="4.0" diaginertia="0.0061063308908 0.0061063308908 0.01125"/>
+      <geom quat="-1 0 0 0" material="UR20_DIFF_8bit_2K_png" mesh="base" class="visual"/>
+      <geom quat="-1 0 0 0" mesh="base" class="collision"/>
       <body name="shoulder_link" pos="0 0 0.2363" quat="0 0 0 1">
-        <inertial
-          pos="0 -0.0062 -0.061"
-          quat="0.843524 0.480626 0.117639 -0.208871"
-          mass="16.343"
-          diaginertia="0.0887394 0.0883735 0.0720472"
-        />
-        <joint name="shoulder_pan_joint" class="size4" />
-        <geom
-          quat="0 0 0 1"
-          material="UR20_DIFF_8bit_2K_png"
-          mesh="shoulder"
-          class="visual"
-        />
-        <geom quat="0 0 0 1" mesh="shoulder" class="collision" />
+        <inertial pos="0 -0.0062 -0.061" quat="0.843524 0.480626 0.117639 -0.208871" mass="16.343" diaginertia="0.0887394 0.0883735 0.0720472"/>
+        <joint name="shoulder_pan_joint" class="size4"/>
+        <geom quat="0 0 0 1" material="UR20_DIFF_8bit_2K_png" mesh="shoulder" class="visual"/>
+        <geom quat="0 0 0 1" mesh="shoulder" class="collision"/>
         <body name="upper_arm_link" quat="0.707107 0.707107 0 0">
-          <inertial
-            pos="-0.3394 0 0.2098"
-            quat="0.502566 0.497435 0.496798 0.503168"
-            mass="29.632"
-            diaginertia="4.6659 4.63539 0.146106"
-          />
-          <joint name="shoulder_lift_joint" class="size4" />
-          <geom
-            pos="0 0 0.26"
-            quat="0.5 0.5 -0.5 -0.5"
-            mesh="upperarm_0"
-            material="UR20_DIFF_8bit_2K_png"
-            class="visual"
-          />
-          <geom
-            pos="0 0 0.26"
-            quat="0.5 0.5 -0.5 -0.5"
-            mesh="upperarm_1"
-            material="UR20_DIFF_8bit_2K_png"
-            class="visual"
-          />
-          <geom
-            pos="0 0 0.26"
-            quat="0.5 0.5 -0.5 -0.5"
-            mesh="upperarm_0"
-            class="collision"
-          />
-          <geom
-            pos="0 0 0.26"
-            quat="0.5 0.5 -0.5 -0.5"
-            mesh="upperarm_1"
-            class="collision"
-          />
+          <inertial pos="-0.3394 0 0.2098" quat="0.502566 0.497435 0.496798 0.503168" mass="29.632" diaginertia="4.6659 4.63539 0.146106"/>
+          <joint name="shoulder_lift_joint" class="size4"/>
+          <geom pos="0 0 0.26" quat="0.5 0.5 -0.5 -0.5" mesh="upperarm_0" material="UR20_DIFF_8bit_2K_png" class="visual"/>
+          <geom pos="0 0 0.26" quat="0.5 0.5 -0.5 -0.5" mesh="upperarm_1" material="UR20_DIFF_8bit_2K_png" class="visual"/>
+          <geom pos="0 0 0.26" quat="0.5 0.5 -0.5 -0.5" mesh="upperarm_0" class="collision"/>
+          <geom pos="0 0 0.26" quat="0.5 0.5 -0.5 -0.5" mesh="upperarm_1" class="collision"/>
           <body name="forearm_link" pos="-0.862 0 0">
-            <inertial
-              pos="-0.4053 0 0.0604"
-              quat="0.510462 0.48933 0.490588 0.509223"
-              mass="7.879"
-              diaginertia="0.75763 0.754423 0.0249668"
-            />
-            <joint name="elbow_joint" class="size3_limited" />
-            <geom
-              pos="0 0 0.043"
-              quat="0.5 0.5 -0.5 -0.5"
-              material="UR20_DIFF_8bit_2K_png"
-              mesh="forearm"
-              class="visual"
-            />
-            <geom
-              pos="0 0 0.043"
-              quat="0.5 0.5 -0.5 -0.5"
-              mesh="forearm"
-              class="collision"
-            />
+            <inertial pos="-0.4053 0 0.0604" quat="0.510462 0.48933 0.490588 0.509223" mass="7.879" diaginertia="0.75763 0.754423 0.0249668"/>
+            <joint name="elbow_joint" class="size3_limited"/>
+            <geom pos="0 0 0.043" quat="0.5 0.5 -0.5 -0.5" material="UR20_DIFF_8bit_2K_png" mesh="forearm" class="visual"/>
+            <geom pos="0 0 0.043" quat="0.5 0.5 -0.5 -0.5" mesh="forearm" class="collision"/>
             <body name="wrist_1_link" pos="-0.7287 0 0.201">
-              <inertial
-                pos="0 -0.0393 -0.0026"
-                quat="0.989529 0.121355 0.0147064 -0.0767408"
-                mass="3.054"
-                diaginertia="0.00555247 0.00545771 0.00392982"
-              />
-              <joint name="wrist_1_joint" class="size2" />
-              <geom
-                pos="0 0 -0.0775"
-                quat="0.707107 0.707107 0 0"
-                material="UR20_DIFF_8bit_2K_png"
-                mesh="wrist1"
-                class="visual"
-              />
-              <geom
-                pos="0 0 -0.0775"
-                quat="0.707107 0.707107 0 0"
-                mesh="wrist1"
-                class="collision"
-              />
-              <body
-                name="wrist_2_link"
-                pos="0 -0.1593 -3.2673e-11"
-                quat="0.707107 0.707107 0 0"
-              >
-                <inertial
-                  pos="0 0.0379 -0.0024"
-                  quat="0.489074 -0.0502817 0.102168 0.864777"
-                  mass="3.126"
-                  diaginertia="0.00586894 0.00585713 0.00418393"
-                />
-                <joint name="wrist_2_joint" axis="0 0 1" class="size2" />
-                <geom
-                  pos="0 0 -0.0749"
-                  quat="1 0 0 0"
-                  material="UR20_DIFF_8bit_2K_png"
-                  mesh="wrist2"
-                  class="visual"
-                />
-                <geom
-                  pos="0 0 -0.0749"
-                  quat="1 0 0 0"
-                  mesh="wrist2"
-                  class="collision"
-                />
-                <body
-                  name="wrist_3_link"
-                  pos="0 0.1543 -3.16475e-11"
-                  quat="0.707107 -0.707107 0 0"
-                >
-                  <inertial
-                    pos="0 0 -0.0293"
-                    quat="0.5 0.5 -0.5 0.5"
-                    mass="0.926"
-                    diaginertia="0.00117 0.00092 0.00091"
-                  />
-                  <joint name="wrist_3_joint" class="size2" />
-                  <geom
-                    pos="0 0 -0.07"
-                    quat="0.707107 0.707107 0 0"
-                    material="UR20_DIFF_8bit_2K_png"
-                    mesh="wrist3"
-                    class="visual"
-                  />
-                  <geom
-                    pos="0 0 -0.07"
-                    quat="0.707107 0.707107 0 0"
-                    mesh="wrist3"
-                    class="collision"
-                  />
+              <inertial pos="0 -0.0393 -0.0026" quat="0.989529 0.121355 0.0147064 -0.0767408" mass="3.054" diaginertia="0.00555247 0.00545771 0.00392982"/>
+              <joint name="wrist_1_joint" class="size2"/>
+              <geom pos="0 0 -0.0775" quat="0.707107 0.707107 0 0" material="UR20_DIFF_8bit_2K_png" mesh="wrist1" class="visual"/>
+              <geom pos="0 0 -0.0775" quat="0.707107 0.707107 0 0" mesh="wrist1" class="collision"/>
+              <body name="wrist_2_link" pos="0 -0.1593 -3.2673e-11" quat="0.707107 0.707107 0 0">
+                <inertial pos="0 0.0379 -0.0024" quat="0.489074 -0.0502817 0.102168 0.864777" mass="3.126" diaginertia="0.00586894 0.00585713 0.00418393"/>
+                <joint name="wrist_2_joint" axis="0 0 1" class="size2"/>
+                <geom pos="0 0 -0.0749" quat="1 0 0 0" material="UR20_DIFF_8bit_2K_png" mesh="wrist2" class="visual"/>
+                <geom pos="0 0 -0.0749" quat="1 0 0 0" mesh="wrist2" class="collision"/>
+                <body name="wrist_3_link" pos="0 0.1543 -3.16475e-11" quat="0.707107 -0.707107 0 0">
+                  <inertial pos="0 0 -0.0293" quat="0.5 0.5 -0.5 0.5" mass="0.926" diaginertia="0.00117 0.00092 0.00091"/>
+                  <joint name="wrist_3_joint" class="size2"/>
+                  <geom pos="0 0 -0.07" quat="0.707107 0.707107 0 0" material="UR20_DIFF_8bit_2K_png" mesh="wrist3" class="visual"/>
+                  <geom pos="0 0 -0.07" quat="0.707107 0.707107 0 0" mesh="wrist3" class="collision"/>
                   <!-- Tool -->
                   <body name="tool_base" pos="0 0 0" gravcomp="1">
-                    <geom
-                      mesh="tool_base"
-                      class="visual"
-                      rgba="0.429 0.429 0.3575 1"
-                    />
-                    <geom mesh="base_geometry_0" class="collision" />
-                    <geom mesh="base_geometry_1" class="collision" />
-                    <geom mesh="base_geometry_2" class="collision" />
-                    <geom mesh="base_geometry_3" class="collision" />
-                    <geom mesh="base_geometry_4" class="collision" />
-                    <geom mesh="base_geometry_5" class="collision" />
-                    <geom mesh="base_geometry_6" class="collision" />
-                    <geom mesh="base_geometry_7" class="collision" />
+                    <geom mesh="tool_base" class="visual" rgba="0.429 0.429 0.3575 1"/>
+                    <geom mesh="base_geometry_0" class="collision"/>
+                    <geom mesh="base_geometry_1" class="collision"/>
+                    <geom mesh="base_geometry_2" class="collision"/>
+                    <geom mesh="base_geometry_3" class="collision"/>
+                    <geom mesh="base_geometry_4" class="collision"/>
+                    <geom mesh="base_geometry_5" class="collision"/>
+                    <geom mesh="base_geometry_6" class="collision"/>
+                    <geom mesh="base_geometry_7" class="collision"/>
                     <body name="tool_tool" pos="0 0 0" gravcomp="1">
-                      <joint
-                        name="tool_slide_joint"
-                        type="slide"
-                        axis="0 1 0"
-                        pos="0 0 0.1"
-                        range="-0.025 0.025"
-                      />
-                      <geom
-                        mesh="tool0_tool"
-                        class="visual"
-                        rgba="0.244 0.244 0.208 1"
-                      />
-                      <geom
-                        mesh="tool0_tool_1"
-                        class="visual"
-                        rgba="0.66 0.66 0.55 1"
-                      />
-                      <geom
-                        mesh="tool0_tool_2"
-                        class="visual"
-                        rgba="0.1 0.1 0.1 1"
-                      />
-                      <geom
-                        mesh="tool0_tool_3"
-                        class="visual"
-                        rgba="0.429 0.429 0.3575 1"
-                      />
-                      <geom mesh="tool0_tool_4" class="visual" />
-                      <geom mesh="tool_geometry_0" class="collision" />
-                      <geom mesh="tool_geometry_1" class="collision" />
-                      <geom mesh="tool_geometry_2" class="collision" />
-                      <geom mesh="tool_geometry_3" class="collision" />
-                      <geom mesh="tool_geometry_4" class="collision" />
-                      <geom mesh="tool_geometry_5" class="collision" />
-                      <geom mesh="tool_geometry_6" class="collision" />
+                      <joint name="tool_slide_joint" type="slide" axis="0 1 0" pos="0 0 0.1" range="-0.025 0.025"/>
+                      <geom mesh="tool0_tool" class="visual" rgba="0.244 0.244 0.208 1"/>
+                      <geom mesh="tool0_tool_1" class="visual" rgba="0.66 0.66 0.55 1"/>
+                      <geom mesh="tool0_tool_2" class="visual" rgba="0.1 0.1 0.1 1"/>
+                      <geom mesh="tool0_tool_3" class="visual" rgba="0.429 0.429 0.3575 1"/>
+                      <geom mesh="tool0_tool_4" class="visual"/>
+                      <geom mesh="tool_geometry_0" class="collision"/>
+                      <geom mesh="tool_geometry_1" class="collision"/>
+                      <geom mesh="tool_geometry_2" class="collision"/>
+                      <geom mesh="tool_geometry_3" class="collision"/>
+                      <geom mesh="tool_geometry_4" class="collision"/>
+                      <geom mesh="tool_geometry_5" class="collision"/>
+                      <geom mesh="tool_geometry_6" class="collision"/>
                     </body>
                   </body>
                 </body>
@@ -357,35 +192,18 @@
 
   <actuator>
     <!-- Robot Actuators -->
-    <position
-      class="high_gain"
-      name="shoulder_pan_joint"
-      joint="shoulder_pan_joint"
-    />
-    <position
-      class="high_gain"
-      name="shoulder_lift_joint"
-      joint="shoulder_lift_joint"
-    />
-    <position class="high_gain" name="elbow_joint" joint="elbow_joint" />
-    <position class="high_gain" name="wrist_1_joint" joint="wrist_1_joint" />
-    <position class="high_gain" name="wrist_2_joint" joint="wrist_2_joint" />
-    <position class="high_gain" name="wrist_3_joint" joint="wrist_3_joint" />
+    <position class="high_gain" name="shoulder_pan_joint" joint="shoulder_pan_joint"/>
+    <position class="high_gain" name="shoulder_lift_joint" joint="shoulder_lift_joint"/>
+    <position class="high_gain" name="elbow_joint" joint="elbow_joint"/>
+    <position class="high_gain" name="wrist_1_joint" joint="wrist_1_joint"/>
+    <position class="high_gain" name="wrist_2_joint" joint="wrist_2_joint"/>
+    <position class="high_gain" name="wrist_3_joint" joint="wrist_3_joint"/>
     <!-- Tool Actuator -->
     <!-- gearing puts this force actuator in units of lbf -->
-    <general
-      name="tool_slide_joint"
-      joint="tool_slide_joint"
-      gear="4.44822"
-      ctrlrange="-150 150"
-    />
+    <general name="tool_slide_joint" joint="tool_slide_joint" gear="4.44822" ctrlrange="-150 150"/>
   </actuator>
 
   <keyframe>
-    <key
-      name="retract"
-      qpos="1.57055 -0.999904 -1.80019 -2.45625 -1.57086 -3.14161 0.0256934"
-      ctrl="1.57055 -0.999904 -1.80019 -2.45625 -1.57086 -3.14161 115"
-    />
+    <key name="retract" qpos="1.57055 -0.999904 -1.80019 -2.45625 -1.57086 -3.14161 0.0256934" ctrl="1.57055 -0.999904 -1.80019 -2.45625 -1.57086 -3.14161 115"/>
   </keyframe>
 </mujoco>
diff --git a/src/grinding_sim/description/tool0.xml b/src/grinding_sim/description/tool0.xml
index 68648cb7..f1f9ffa7 100644
--- a/src/grinding_sim/description/tool0.xml
+++ b/src/grinding_sim/description/tool0.xml
@@ -1,114 +1,81 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="tool0">
-  <compiler angle="radian" autolimits="true" meshdir="assets" />
+  <compiler angle="radian" autolimits="true" meshdir="assets"/>
 
   <default>
     <!-- Geom defaults -->
     <default class="visual">
-      <geom group="2" type="mesh" contype="0" conaffinity="0" />
+      <geom group="2" type="mesh" contype="0" conaffinity="0"/>
     </default>
     <default class="collision">
-      <geom group="3" type="mesh" />
+      <geom group="3" type="mesh"/>
     </default>
   </default>
 
   <asset>
-    <texture
-      type="2d"
-      name="groundplane"
-      builtin="checker"
-      mark="edge"
-      rgb1="0.2 0.3 0.4"
-      rgb2="0.1 0.2 0.3"
-      markrgb="0.8 0.8 0.8"
-      width="300"
-      height="300"
-    />
-    <material
-      name="groundplane"
-      texture="groundplane"
-      texuniform="true"
-      texrepeat="5 5"
-      reflectance="0.2"
-    />
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
     <!-- Tool-Base geometries -->
-    <mesh file="tool_base/tool_base.obj" />
-    <mesh name="base_geometry_0" file="tool_base/geometry_0.stl" />
-    <mesh name="base_geometry_1" file="tool_base/geometry_1.stl" />
-    <mesh name="base_geometry_2" file="tool_base/geometry_2.stl" />
-    <mesh name="base_geometry_3" file="tool_base/geometry_3.stl" />
-    <mesh name="base_geometry_4" file="tool_base/geometry_4.stl" />
-    <mesh name="base_geometry_5" file="tool_base/geometry_5.stl" />
-    <mesh name="base_geometry_6" file="tool_base/geometry_6.stl" />
-    <mesh name="base_geometry_7" file="tool_base/geometry_7.stl" />
+    <mesh file="tool_base/tool_base.obj"/>
+    <mesh name="base_geometry_0" file="tool_base/geometry_0.stl"/>
+    <mesh name="base_geometry_1" file="tool_base/geometry_1.stl"/>
+    <mesh name="base_geometry_2" file="tool_base/geometry_2.stl"/>
+    <mesh name="base_geometry_3" file="tool_base/geometry_3.stl"/>
+    <mesh name="base_geometry_4" file="tool_base/geometry_4.stl"/>
+    <mesh name="base_geometry_5" file="tool_base/geometry_5.stl"/>
+    <mesh name="base_geometry_6" file="tool_base/geometry_6.stl"/>
+    <mesh name="base_geometry_7" file="tool_base/geometry_7.stl"/>
 
     <!-- Tool-Tool geometries -->
-    <mesh file="tool0_tool/tool0_tool.obj" />
-    <mesh name="tool_geometry_0" file="tool0_tool/geometry_0.stl" />
-    <mesh name="tool_geometry_1" file="tool0_tool/geometry_1.stl" />
-    <mesh name="tool_geometry_2" file="tool0_tool/geometry_2.stl" />
-    <mesh name="tool_geometry_3" file="tool0_tool/geometry_3.stl" />
-    <mesh name="tool_geometry_4" file="tool0_tool/geometry_4.stl" />
-    <mesh name="tool_geometry_5" file="tool0_tool/geometry_5.stl" />
-    <mesh name="tool_geometry_6" file="tool0_tool/geometry_6.stl" />
+    <mesh file="tool0_tool/tool0_tool.obj"/>
+    <mesh name="tool_geometry_0" file="tool0_tool/geometry_0.stl"/>
+    <mesh name="tool_geometry_1" file="tool0_tool/geometry_1.stl"/>
+    <mesh name="tool_geometry_2" file="tool0_tool/geometry_2.stl"/>
+    <mesh name="tool_geometry_3" file="tool0_tool/geometry_3.stl"/>
+    <mesh name="tool_geometry_4" file="tool0_tool/geometry_4.stl"/>
+    <mesh name="tool_geometry_5" file="tool0_tool/geometry_5.stl"/>
+    <mesh name="tool_geometry_6" file="tool0_tool/geometry_6.stl"/>
   </asset>
 
   <worldbody>
-    <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
-    <geom
-      name="floor"
-      pos="0 0 0"
-      size="0 0 0.05"
-      type="plane"
-      material="groundplane"
-    />
+    <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
+    <geom name="floor" pos="0 0 0" size="0 0 0.05" type="plane" material="groundplane"/>
 
     <!-- Tool -->
     <body name="tool_base" pos="0 0 0.1" quat="1 0 0 0">
-      <geom quat="1 0 0 0" mesh="tool_base" class="visual" />
-      <geom quat="1 0 0 0" mesh="base_geometry_0" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_1" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_2" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_3" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_4" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_5" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_6" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_7" class="collision" />
+      <geom quat="1 0 0 0" mesh="tool_base" class="visual"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_0" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_1" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_2" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_3" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_4" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_5" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_6" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_7" class="collision"/>
       <body name="tool_tool" pos="0 0 0" quat="-1 0 0 0">
-        <joint
-          name="tool_slide_joint"
-          type="slide"
-          axis="0 1 0"
-          pos="0 0 0.1"
-          range="-0.015 0.015"
-        />
-        <geom quat="1 0 0 0" mesh="tool0_tool" class="visual" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_0" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_1" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_2" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_3" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_4" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_5" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_6" class="collision" />
+        <joint name="tool_slide_joint" type="slide" axis="0 1 0" pos="0 0 0.1" range="-0.015 0.015"/>
+        <geom quat="1 0 0 0" mesh="tool0_tool" class="visual"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_0" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_1" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_2" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_3" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_4" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_5" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_6" class="collision"/>
       </body>
     </body>
   </worldbody>
 
   <actuator>
     <!-- gearing puts this force actuator in units of lbf -->
-    <general
-      name="tool_slide_joint"
-      joint="tool_slide_joint"
-      gear="4.44822"
-      ctrlrange="-150 150"
-      user="115"
-    />
+    <general name="tool_slide_joint" joint="tool_slide_joint" gear="4.44822" ctrlrange="-150 150" user="115"/>
   </actuator>
 
   <contact>
-    <exclude name="tool_contact_exclude" body1="tool_base" body2="tool_tool" />
+    <exclude name="tool_contact_exclude" body1="tool_base" body2="tool_tool"/>
   </contact>
 
   <keyframe>
-    <key name="default" qpos="0" ctrl="115" />
+    <key name="default" qpos="0" ctrl="115"/>
   </keyframe>
 </mujoco>
diff --git a/src/grinding_sim/description/tool90.xml b/src/grinding_sim/description/tool90.xml
index 9d949158..0619c855 100644
--- a/src/grinding_sim/description/tool90.xml
+++ b/src/grinding_sim/description/tool90.xml
@@ -1,115 +1,82 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="tool90">
-  <compiler angle="radian" autolimits="true" meshdir="assets" />
+  <compiler angle="radian" autolimits="true" meshdir="assets"/>
 
   <default>
     <!-- Geom defaults -->
     <default class="visual">
-      <geom group="2" type="mesh" contype="0" conaffinity="0" />
+      <geom group="2" type="mesh" contype="0" conaffinity="0"/>
     </default>
     <default class="collision">
-      <geom group="3" type="mesh" />
+      <geom group="3" type="mesh"/>
     </default>
   </default>
 
   <asset>
-    <texture
-      type="2d"
-      name="groundplane"
-      builtin="checker"
-      mark="edge"
-      rgb1="0.2 0.3 0.4"
-      rgb2="0.1 0.2 0.3"
-      markrgb="0.8 0.8 0.8"
-      width="300"
-      height="300"
-    />
-    <material
-      name="groundplane"
-      texture="groundplane"
-      texuniform="true"
-      texrepeat="5 5"
-      reflectance="0.2"
-    />
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
     <!-- Base geometries -->
-    <mesh file="tool_base/tool_base.obj" />
-    <mesh name="base_geometry_0" file="tool_base/geometry_0.stl" />
-    <mesh name="base_geometry_1" file="tool_base/geometry_1.stl" />
-    <mesh name="base_geometry_2" file="tool_base/geometry_2.stl" />
-    <mesh name="base_geometry_3" file="tool_base/geometry_3.stl" />
-    <mesh name="base_geometry_4" file="tool_base/geometry_4.stl" />
-    <mesh name="base_geometry_5" file="tool_base/geometry_5.stl" />
-    <mesh name="base_geometry_6" file="tool_base/geometry_6.stl" />
-    <mesh name="base_geometry_7" file="tool_base/geometry_7.stl" />
+    <mesh file="tool_base/tool_base.obj"/>
+    <mesh name="base_geometry_0" file="tool_base/geometry_0.stl"/>
+    <mesh name="base_geometry_1" file="tool_base/geometry_1.stl"/>
+    <mesh name="base_geometry_2" file="tool_base/geometry_2.stl"/>
+    <mesh name="base_geometry_3" file="tool_base/geometry_3.stl"/>
+    <mesh name="base_geometry_4" file="tool_base/geometry_4.stl"/>
+    <mesh name="base_geometry_5" file="tool_base/geometry_5.stl"/>
+    <mesh name="base_geometry_6" file="tool_base/geometry_6.stl"/>
+    <mesh name="base_geometry_7" file="tool_base/geometry_7.stl"/>
 
     <!-- Tool geometries -->
-    <mesh file="tool90_tool/tool90_tool.obj" />
-    <mesh name="tool_geometry_0" file="tool90_tool/geometry_0.stl" />
-    <mesh name="tool_geometry_1" file="tool90_tool/geometry_1.stl" />
-    <mesh name="tool_geometry_2" file="tool90_tool/geometry_2.stl" />
-    <mesh name="tool_geometry_3" file="tool90_tool/geometry_3.stl" />
-    <mesh name="tool_geometry_4" file="tool90_tool/geometry_4.stl" />
-    <mesh name="tool_geometry_5" file="tool90_tool/geometry_5.stl" />
-    <mesh name="tool_geometry_6" file="tool90_tool/geometry_6.stl" />
-    <mesh name="tool_geometry_7" file="tool90_tool/geometry_7.stl" />
-    <mesh name="tool_geometry_8" file="tool90_tool/geometry_8.stl" />
-    <mesh name="tool_geometry_9" file="tool90_tool/geometry_9.stl" />
-    <mesh name="tool_geometry_10" file="tool90_tool/geometry_10.stl" />
-    <mesh name="tool_geometry_11" file="tool90_tool/geometry_11.stl" />
+    <mesh file="tool90_tool/tool90_tool.obj"/>
+    <mesh name="tool_geometry_0" file="tool90_tool/geometry_0.stl"/>
+    <mesh name="tool_geometry_1" file="tool90_tool/geometry_1.stl"/>
+    <mesh name="tool_geometry_2" file="tool90_tool/geometry_2.stl"/>
+    <mesh name="tool_geometry_3" file="tool90_tool/geometry_3.stl"/>
+    <mesh name="tool_geometry_4" file="tool90_tool/geometry_4.stl"/>
+    <mesh name="tool_geometry_5" file="tool90_tool/geometry_5.stl"/>
+    <mesh name="tool_geometry_6" file="tool90_tool/geometry_6.stl"/>
+    <mesh name="tool_geometry_7" file="tool90_tool/geometry_7.stl"/>
+    <mesh name="tool_geometry_8" file="tool90_tool/geometry_8.stl"/>
+    <mesh name="tool_geometry_9" file="tool90_tool/geometry_9.stl"/>
+    <mesh name="tool_geometry_10" file="tool90_tool/geometry_10.stl"/>
+    <mesh name="tool_geometry_11" file="tool90_tool/geometry_11.stl"/>
   </asset>
 
   <worldbody>
-    <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
-    <geom
-      name="floor"
-      pos="0 0 0"
-      size="0 0 0.05"
-      type="plane"
-      material="groundplane"
-    />
+    <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
+    <geom name="floor" pos="0 0 0" size="0 0 0.05" type="plane" material="groundplane"/>
 
     <!-- Tool -->
     <body name="tool_base" pos="0 0 0" quat="1 0 0 0">
-      <geom quat="1 0 0 0" mesh="tool_base" class="visual" />
-      <geom quat="1 0 0 0" mesh="base_geometry_0" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_1" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_2" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_3" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_4" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_5" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_6" class="collision" />
-      <geom quat="1 0 0 0" mesh="base_geometry_7" class="collision" />
+      <geom quat="1 0 0 0" mesh="tool_base" class="visual"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_0" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_1" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_2" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_3" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_4" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_5" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_6" class="collision"/>
+      <geom quat="1 0 0 0" mesh="base_geometry_7" class="collision"/>
       <body name="tool_tool" pos="0 0 0" quat="1 0 0 0">
-        <joint
-          name="tool_joint"
-          type="slide"
-          axis="0 1 0"
-          pos="0 0 0.1"
-          range="-0.015 0.015"
-        />
-        <geom quat="1 0 0 0" mesh="tool90_tool" class="visual" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_0" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_1" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_2" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_3" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_4" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_5" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_6" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_7" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_8" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_9" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_10" class="collision" />
-        <geom quat="1 0 0 0" mesh="tool_geometry_11" class="collision" />
+        <joint name="tool_joint" type="slide" axis="0 1 0" pos="0 0 0.1" range="-0.015 0.015"/>
+        <geom quat="1 0 0 0" mesh="tool90_tool" class="visual"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_0" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_1" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_2" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_3" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_4" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_5" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_6" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_7" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_8" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_9" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_10" class="collision"/>
+        <geom quat="1 0 0 0" mesh="tool_geometry_11" class="collision"/>
       </body>
     </body>
   </worldbody>
 
   <actuator>
-    <position
-      name="tool_center"
-      joint="tool_joint"
-      kp="500000"
-      kv="100"
-      ctrlrange="-0.025 0.025"
-    />
+    <position name="tool_center" joint="tool_joint" kp="500000" kv="100" ctrlrange="-0.025 0.025"/>
   </actuator>
 </mujoco>
diff --git a/src/grinding_sim/launch/agent_bridge.launch.xml b/src/grinding_sim/launch/agent_bridge.launch.xml
index b2aa7543..8eeec7d9 100644
--- a/src/grinding_sim/launch/agent_bridge.launch.xml
+++ b/src/grinding_sim/launch/agent_bridge.launch.xml
@@ -1,6 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
 </launch>
diff --git a/src/grinding_sim/objectives/capture_point_cloud.xml b/src/grinding_sim/objectives/capture_point_cloud.xml
index e04d9491..12ae568b 100644
--- a/src/grinding_sim/objectives/capture_point_cloud.xml
+++ b/src/grinding_sim/objectives/capture_point_cloud.xml
@@ -1,50 +1,20 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Capture Point Cloud">
   <!--//////////-->
-  <BehaviorTree
-    ID="Capture Point Cloud"
-    _description=""
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Capture Point Cloud" _description="" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="GetPointCloud"
-        timeout_sec="5.000000"
-        topic_name="/scene_camera/points"
-        message_out="{point_cloud}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="world"
-        stamped_pose="{stamped_pose}"
-        position_xyz="0;-1.69;1.0"
-      />
-      <Action
-        ID="CropPointsInBox"
-        crop_box_centroid_pose="{stamped_pose}"
-        point_cloud="{point_cloud}"
-        point_cloud_cropped="{point_cloud_cropped}"
-        crop_box_size="1.0;0.5;0.55"
-      />
-      <Action
-        ID="SendPointCloudToUI"
-        point_cloud="{point_cloud_cropped}"
-        pcd_topic="/pcd_pointcloud_captures"
-      />
-      <Action
-        ID="SavePointCloudToFile"
-        file_path="~/.config/moveit_pro/saved_behavior_data"
-        file_prefix="engine_block"
-        point_cloud="{point_cloud_cropped}"
-      />
+      <Action ID="GetPointCloud" timeout_sec="5.000000" topic_name="/scene_camera/points" message_out="{point_cloud}"/>
+      <Action ID="CreateStampedPose" reference_frame="world" stamped_pose="{stamped_pose}" position_xyz="0;-1.69;1.0"/>
+      <Action ID="CropPointsInBox" crop_box_centroid_pose="{stamped_pose}" point_cloud="{point_cloud}" point_cloud_cropped="{point_cloud_cropped}" crop_box_size="1.0;0.5;0.55"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_cropped}" pcd_topic="/pcd_pointcloud_captures"/>
+      <Action ID="SavePointCloudToFile" file_path="~/.config/moveit_pro/saved_behavior_data" file_prefix="engine_block" point_cloud="{point_cloud_cropped}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Capture Point Cloud">
       <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Perception - 3D Point Cloud"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/grinding_sim/objectives/grind_machined_part.xml b/src/grinding_sim/objectives/grind_machined_part.xml
index 60ab1698..9cd3ade6 100644
--- a/src/grinding_sim/objectives/grind_machined_part.xml
+++ b/src/grinding_sim/objectives/grind_machined_part.xml
@@ -1,141 +1,54 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Grind Machined Part">
   <!--//////////-->
-  <BehaviorTree
-    ID="Grind Machined Part"
-    _description=""
-    _favorite="true"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Grind Machined Part" _description="" _favorite="true" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="ClearSnapshot" />
+      <Action ID="ClearSnapshot"/>
       <!--View workspace and register part-->
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        waypoint_name="Retract"
-        name="View Workspace"
-      />
-      <SubTree
-        ID="Register Machined Part"
-        _collapsed="true"
-        registered_pose="{registered_pose}"
-      />
-      <SubTree ID="Take Snapshot" _collapsed="true" />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0" waypoint_name="Retract" name="View Workspace"/>
+      <SubTree ID="Register Machined Part" _collapsed="true" registered_pose="{registered_pose}"/>
+      <SubTree ID="Take Snapshot" _collapsed="true"/>
       <!--Publish frame of registered part so we can plan in reference to it-->
       <Control ID="Fallback">
         <Decorator ID="Timeout" msec="500">
-          <Action
-            ID="PublishStaticFrame"
-            pose="{registered_pose}"
-            publish_rate="50"
-            child_frame_id="registered_pose"
-          />
+          <Action ID="PublishStaticFrame" pose="{registered_pose}" publish_rate="50" child_frame_id="registered_pose"/>
         </Decorator>
-        <Action ID="AlwaysSuccess" />
+        <Action ID="AlwaysSuccess"/>
       </Control>
       <!--Move the tool to a pose near the part-->
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        waypoint_name="Pre Grind"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0" waypoint_name="Pre Grind"/>
       <!--Load grinding poses-->
-      <Action
-        ID="LoadPoseStampedVectorFromYaml"
-        output="{grinding_poses}"
-        file_path="grinding_poses.yaml"
-      />
+      <Action ID="LoadPoseStampedVectorFromYaml" output="{grinding_poses}" file_path="grinding_poses.yaml"/>
       <!--Visualize grinding poses-->
-      <Action ID="Script" code="pose_count := 0" />
-      <Decorator
-        ID="ForEach"
-        vector_in="{grinding_poses}"
-        out="{target_pose}"
-        index="{index}"
-      >
+      <Action ID="Script" code="pose_count := 0"/>
+      <Decorator ID="ForEach" vector_in="{grinding_poses}" out="{target_pose}" index="{index}">
         <Control ID="Sequence">
-          <Action
-            ID="VisualizePose"
-            marker_name="{pose_count}"
-            pose="{target_pose}"
-            marker_lifetime="0.000000"
-            marker_size="0.100000"
-          />
-          <Action ID="Script" code="pose_count += 1" />
+          <Action ID="VisualizePose" marker_name="{pose_count}" pose="{target_pose}" marker_lifetime="0.000000" marker_size="0.100000"/>
+          <Action ID="Script" code="pose_count += 1"/>
         </Control>
       </Decorator>
       <!--Plan a Cartesian path that moves the tool tip through the grinding poses-->
-      <Action
-        ID="PlanCartesianPath"
-        acceleration_scale_factor="1.000000"
-        blending_radius="0.020000"
-        ik_cartesian_space_density="0.010000"
-        ik_joint_space_density="0.100000"
-        path="{grinding_poses}"
-        planning_group_name="manipulator"
-        position_only="false"
-        tip_link="tool_tip"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="0.01"
-        debug_solution="{debug_solution}"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <Action ID="PlanCartesianPath" acceleration_scale_factor="1.000000" blending_radius="0.020000" ik_cartesian_space_density="0.010000" ik_joint_space_density="0.100000" path="{grinding_poses}" planning_group_name="manipulator" position_only="false" tip_link="tool_tip" trajectory_sampling_rate="100" velocity_scale_factor="0.01" debug_solution="{debug_solution}" joint_trajectory_msg="{joint_trajectory_msg}"/>
       <!--Wait for user to approve the grinding path and then execute it-->
-      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-        goal_duration_tolerance="-1.000000"
-        goal_position_tolerance="0.000000"
-        goal_time_tolerance="0.000000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
       <!--Move away from the part after completing the grinding path-->
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        waypoint_name="Approach"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0" waypoint_name="Approach"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Grind Machined Part">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Grinding" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Application - Grinding"/>
       </MetadataFields>
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Grinding" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Application - Grinding"/>
       </MetadataFields>
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Grinding" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Application - Grinding"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/grinding_sim/objectives/load_engine_block_pointcloud.xml b/src/grinding_sim/objectives/load_engine_block_pointcloud.xml
index 36b6983d..ecbcc517 100644
--- a/src/grinding_sim/objectives/load_engine_block_pointcloud.xml
+++ b/src/grinding_sim/objectives/load_engine_block_pointcloud.xml
@@ -1,42 +1,20 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Load Engine Block Pointcloud">
   <!--//////////-->
-  <BehaviorTree
-    ID="Load Engine Block Pointcloud"
-    _description=""
-    _favorite="false"
-    _subtreeOnly="true"
-    color="255;0;0"
-    initial_pose="{initial_pose}"
-    engine_block_cloud="{engine_block_cloud}"
-  >
+  <BehaviorTree ID="Load Engine Block Pointcloud" _description="" _favorite="false" _subtreeOnly="true" color="255;0;0" initial_pose="{initial_pose}" engine_block_cloud="{engine_block_cloud}">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="LoadPointCloudFromFile"
-        color="{color}"
-        package_name="picknik_accessories"
-        file_path="mujoco_assets/grinding_sim_assets/engine_block/engine_block.pcd"
-        point_cloud="{engine_block_cloud}"
-        frame_id="world"
-        num_sampled_points="10000"
-        scale="1.000000"
-      />
-      <Action
-        ID="TransformPointCloud"
-        input_cloud="{engine_block_cloud}"
-        transform_pose="{initial_pose}"
-        output_cloud="{engine_block_cloud}"
-      />
+      <Action ID="LoadPointCloudFromFile" color="{color}" package_name="picknik_accessories" file_path="mujoco_assets/grinding_sim_assets/engine_block/engine_block.pcd" point_cloud="{engine_block_cloud}" frame_id="world" num_sampled_points="10000" scale="1.000000"/>
+      <Action ID="TransformPointCloud" input_cloud="{engine_block_cloud}" transform_pose="{initial_pose}" output_cloud="{engine_block_cloud}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Load Engine Block Pointcloud">
-      <input_port name="color" default="255;0;0" />
-      <input_port name="initial_pose" default="{initial_pose}" />
-      <output_port name="engine_block_cloud" default="{engine_block_cloud}" />
+      <input_port name="color" default="255;0;0"/>
+      <input_port name="initial_pose" default="{initial_pose}"/>
+      <output_port name="engine_block_cloud" default="{engine_block_cloud}"/>
       <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
-        <Metadata runnable="false" />
+        <Metadata subcategory="Perception - 3D Point Cloud"/>
+        <Metadata runnable="false"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/grinding_sim/objectives/move_to_loaded_pose.xml b/src/grinding_sim/objectives/move_to_loaded_pose.xml
index 69f88f7d..8ec2cafd 100644
--- a/src/grinding_sim/objectives/move_to_loaded_pose.xml
+++ b/src/grinding_sim/objectives/move_to_loaded_pose.xml
@@ -1,34 +1,18 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move To Loaded Pose">
   <!--//////////-->
-  <BehaviorTree
-    ID="Move To Loaded Pose"
-    _description=""
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Move To Loaded Pose" _description="" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="LoadPoseStampedFromYaml"
-        output="{target_pose}"
-        file_path="approach_pose.yaml"
-      />
-      <Action
-        ID="BreakpointSubscriber"
-        breakpoint_topic="/moveit_pro_breakpoint"
-      />
-      <SubTree
-        ID="Move to Pose"
-        _collapsed="false"
-        target_pose="{target_pose}"
-      />
+      <Action ID="LoadPoseStampedFromYaml" output="{target_pose}" file_path="approach_pose.yaml"/>
+      <Action ID="BreakpointSubscriber" breakpoint_topic="/moveit_pro_breakpoint"/>
+      <SubTree ID="Move to Pose" _collapsed="false" target_pose="{target_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move To Loaded Pose">
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Motion - Execute"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/grinding_sim/objectives/move_to_pose.xml b/src/grinding_sim/objectives/move_to_pose.xml
index e4888e1b..29ffed92 100644
--- a/src/grinding_sim/objectives/move_to_pose.xml
+++ b/src/grinding_sim/objectives/move_to_pose.xml
@@ -1,38 +1,19 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Pose">
-  <BehaviorTree
-    ID="Move to Pose"
-    _description="Uses inverse kinematics to move the robot to a set gripper position"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Move to Pose" _description="Uses inverse kinematics to move the robot to a set gripper position" _subtreeOnly="true">
     <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_pose"
-        controller_names="/joint_trajectory_controller"
-        task="{move_to_pose_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
-      <Action
-        ID="SetupMTCPlanToPose"
-        ik_frame="tool_tip"
-        planning_group_name="manipulator"
-        target_pose="{target_pose}"
-        task="{move_to_pose_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_pose_solution}"
-        task="{move_to_pose_task}"
-      />
-      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
+      <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCPlanToPose" ik_frame="tool_tip" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}"/>
+      <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Pose">
-      <input_port name="target_pose" default="{target_pose}" />
+      <input_port name="target_pose" default="{target_pose}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/grinding_sim/objectives/move_to_pose_imarker.xml b/src/grinding_sim/objectives/move_to_pose_imarker.xml
index 2c1b7f73..00199367 100644
--- a/src/grinding_sim/objectives/move_to_pose_imarker.xml
+++ b/src/grinding_sim/objectives/move_to_pose_imarker.xml
@@ -1,58 +1,20 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Pose iMarker">
-  <BehaviorTree
-    ID="Move to Pose iMarker"
-    _description="Uses inverse kinematics to move the robot to a set gripper position"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Move to Pose iMarker" _description="Uses inverse kinematics to move the robot to a set gripper position" _subtreeOnly="true">
     <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_pose"
-        controller_names="/joint_trajectory_controller"
-        task="{move_to_pose_task}"
-      />
-      <Action
-        ID="SetupMTCCurrentState"
-        task="{move_to_pose_task}"
-        skip_collision_check="false"
-      />
-      <Action
-        ID="SetupMTCPlanToPose"
-        ik_frame="tool_tip"
-        planning_group_name="manipulator"
-        target_pose="{target_pose}"
-        task="{move_to_pose_task}"
-        acceleration_scale_factor="1.000000"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.100000"
-        link_padding="0.000000"
-        max_iterations="5000"
-        monitored_stage="current state"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="1.000000"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_pose_solution}"
-        task="{move_to_pose_task}"
-      />
-      <SubTree
-        ID="Wait for Trajectory Approval if User Available"
-        solution="{move_to_pose_solution}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{move_to_pose_solution}"
-        goal_duration_tolerance="-1.000000"
-      />
+      <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" skip_collision_check="false"/>
+      <Action ID="SetupMTCPlanToPose" ik_frame="tool_tip" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" acceleration_scale_factor="1.000000" keep_orientation="false" keep_orientation_tolerance="0.100000" link_padding="0.000000" max_iterations="5000" monitored_stage="current state" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+      <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+      <SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" goal_duration_tolerance="-1.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Pose iMarker">
-      <input_port name="target_pose" default="{target_pose}" />
+      <input_port name="target_pose" default="{target_pose}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/grinding_sim/objectives/register_machined_part.xml b/src/grinding_sim/objectives/register_machined_part.xml
index 9f223421..1bfbd1d8 100644
--- a/src/grinding_sim/objectives/register_machined_part.xml
+++ b/src/grinding_sim/objectives/register_machined_part.xml
@@ -1,110 +1,35 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Register Machined Part">
   <!--//////////-->
-  <BehaviorTree
-    ID="Register Machined Part"
-    _description=""
-    _favorite="true"
-    registered_pose="{registered_pose}"
-  >
+  <BehaviorTree ID="Register Machined Part" _description="" _favorite="true" registered_pose="{registered_pose}">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="ClearSnapshot" />
-      <Action
-        ID="CreateStampedPose"
-        position_xyz="0.2;-1.5;0.710"
-        orientation_xyzw="0.000;0.000;0.3827;0.9239"
-        name="Initial Guess"
-        stamped_pose="{guess_pose}"
-        reference_frame="world"
-      />
-      <SubTree
-        ID="Load Engine Block Pointcloud"
-        _collapsed="true"
-        color="0;0;255"
-        initial_pose="{guess_pose}"
-        engine_block_cloud="{blue_engine_block_cloud}"
-      />
-      <Action
-        ID="SendPointCloudToUI"
-        point_cloud="{blue_engine_block_cloud}"
-        pcd_topic="/pcd_pointcloud_captures"
-      />
-      <Action
-        ID="GetPointCloud"
-        topic_name="/scene_camera/points"
-        message_out="{scene_point_cloud}"
-        timeout_sec="5.000000"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="world"
-        stamped_pose="{centroid_pose}"
-        position_xyz="0;-1.69;1.0"
-      />
-      <Action
-        ID="CropPointsInBox"
-        crop_box_centroid_pose="{centroid_pose}"
-        point_cloud="{scene_point_cloud}"
-        point_cloud_cropped="{cropped_scene_point_cloud}"
-        crop_box_size="1.0;0.5;0.55"
-      />
-      <Action
-        ID="TransformPointCloudFrame"
-        input_cloud="{cropped_scene_point_cloud}"
-        output_cloud="{cropped_scene_point_cloud}"
-        target_frame="world"
-      />
-      <Action
-        ID="SendPointCloudToUI"
-        point_cloud="{cropped_scene_point_cloud}"
-        pcd_topic="/pcd_pointcloud_captures"
-      />
-      <Action
-        ID="RegisterPointClouds"
-        target_point_cloud="{cropped_scene_point_cloud}"
-        base_point_cloud="{blue_engine_block_cloud}"
-        max_iterations="100"
-        target_pose_in_base_frame="{registered_to_guess_pose}"
-        max_correspondence_distance="0.5"
-      />
-      <Action
-        ID="TransformPoseWithPose"
-        input_pose="{guess_pose}"
-        transform_pose="{registered_to_guess_pose}"
-        output_pose="{registered_pose}"
-      />
-      <SubTree
-        ID="Load Engine Block Pointcloud"
-        _collapsed="true"
-        color="0;255;0"
-        initial_pose="{registered_pose}"
-        engine_block_cloud="{green_engine_block_cloud}"
-      />
-      <Action ID="ClearSnapshot" />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="registered_pose"
-        marker_size="0.100000"
-        pose="{registered_pose}"
-      />
-      <Action
-        ID="SendPointCloudToUI"
-        point_cloud="{green_engine_block_cloud}"
-        pcd_topic="/pcd_pointcloud_captures"
-      />
+      <Action ID="ClearSnapshot"/>
+      <Action ID="CreateStampedPose" position_xyz="0.2;-1.5;0.710" orientation_xyzw="0.000;0.000;0.3827;0.9239" name="Initial Guess" stamped_pose="{guess_pose}" reference_frame="world"/>
+      <SubTree ID="Load Engine Block Pointcloud" _collapsed="true" color="0;0;255" initial_pose="{guess_pose}" engine_block_cloud="{blue_engine_block_cloud}"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{blue_engine_block_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
+      <Action ID="GetPointCloud" topic_name="/scene_camera/points" message_out="{scene_point_cloud}" timeout_sec="5.000000"/>
+      <Action ID="CreateStampedPose" reference_frame="world" stamped_pose="{centroid_pose}" position_xyz="0;-1.69;1.0"/>
+      <Action ID="CropPointsInBox" crop_box_centroid_pose="{centroid_pose}" point_cloud="{scene_point_cloud}" point_cloud_cropped="{cropped_scene_point_cloud}" crop_box_size="1.0;0.5;0.55"/>
+      <Action ID="TransformPointCloudFrame" input_cloud="{cropped_scene_point_cloud}" output_cloud="{cropped_scene_point_cloud}" target_frame="world"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{cropped_scene_point_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
+      <Action ID="RegisterPointClouds" target_point_cloud="{cropped_scene_point_cloud}" base_point_cloud="{blue_engine_block_cloud}" max_iterations="100" target_pose_in_base_frame="{registered_to_guess_pose}" max_correspondence_distance="0.5"/>
+      <Action ID="TransformPoseWithPose" input_pose="{guess_pose}" transform_pose="{registered_to_guess_pose}" output_pose="{registered_pose}"/>
+      <SubTree ID="Load Engine Block Pointcloud" _collapsed="true" color="0;255;0" initial_pose="{registered_pose}" engine_block_cloud="{green_engine_block_cloud}"/>
+      <Action ID="ClearSnapshot"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="registered_pose" marker_size="0.100000" pose="{registered_pose}"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{green_engine_block_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Register Machined Part">
-      <output_port name="registered_pose" default="{registered_pose}" />
+      <output_port name="registered_pose" default="{registered_pose}"/>
       <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Perception - 3D Point Cloud"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
       <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Perception - 3D Point Cloud"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/grinding_sim/objectives/request_teleoperation.xml b/src/grinding_sim/objectives/request_teleoperation.xml
index 779d8b91..12b13a1c 100644
--- a/src/grinding_sim/objectives/request_teleoperation.xml
+++ b/src/grinding_sim/objectives/request_teleoperation.xml
@@ -1,23 +1,11 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Request Teleoperation">
   <!--The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message.-->
-  <BehaviorTree
-    ID="Request Teleoperation"
-    _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective."
-    _favorite="false"
-    _hardcoded="false"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Request Teleoperation" _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective." _favorite="false" _hardcoded="false" _subtreeOnly="true">
     <Control ID="Sequence">
-      <Action ID="Script" code="teleop_mode := 0" />
+      <Action ID="Script" code="teleop_mode := 0"/>
       <Control ID="Parallel" success_count="1" failure_count="1">
-        <Action
-          ID="DoTeleoperateAction"
-          enable_user_interaction="{enable_user_interaction}"
-          user_interaction_prompt="{user_interaction_prompt}"
-          initial_teleop_mode="{initial_teleop_mode}"
-          current_teleop_mode="{teleop_mode}"
-        />
+        <Action ID="DoTeleoperateAction" enable_user_interaction="{enable_user_interaction}" user_interaction_prompt="{user_interaction_prompt}" initial_teleop_mode="{initial_teleop_mode}" current_teleop_mode="{teleop_mode}"/>
         <Decorator ID="KeepRunningUntilFailure">
           <Control ID="Sequence">
             <!--Joint sliders interpolate to joint state-->
@@ -25,31 +13,13 @@
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrieveJointStateParameter"
-                      timeout_sec="-1"
-                      joint_state="{target_joint_state}"
-                    />
-                    <SubTree
-                      ID="Interpolate to Joint State"
-                      _collapsed="false"
-                      target_joint_state="{target_joint_state}"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
+                    <SubTree ID="Interpolate to Joint State" _collapsed="false" target_joint_state="{target_joint_state}"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
@@ -59,31 +29,13 @@
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrievePoseParameter"
-                      timeout_sec="-1"
-                      pose="{target_pose}"
-                    />
-                    <SubTree
-                      ID="Move to Pose iMarker"
-                      _collapsed="false"
-                      target_pose="{target_pose}"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrievePoseParameter" timeout_sec="-1" pose="{target_pose}"/>
+                    <SubTree ID="Move to Pose iMarker" _collapsed="false" target_pose="{target_pose}"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
@@ -93,55 +45,23 @@
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrieveJointStateParameter"
-                      timeout_sec="-1"
-                      joint_state="{target_joint_state}"
-                    />
-                    <SubTree
-                      ID="Move to Joint State"
-                      _collapsed="false"
-                      target_joint_state="{target_joint_state}"
-                      acceleration_scale_factor="1.0"
-                      controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-                      controller_names="joint_trajectory_controller"
-                      joint_group_name="manipulator"
-                      keep_orientation="false"
-                      keep_orientation_tolerance="0.05"
-                      link_padding="0.01"
-                      velocity_scale_factor="1.0"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
+                    <SubTree ID="Move to Joint State" _collapsed="false" target_joint_state="{target_joint_state}" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
             </Decorator>
             <!--Cartesian and joint jogging-->
             <Control ID="Sequence" _while="teleop_mode == 2">
-              <Action
-                ID="TeleoperateTwist"
-                controller_name="servo_controller"
-              />
+              <Action ID="TeleoperateTwist" controller_name="servo_controller"/>
             </Control>
             <Control ID="Sequence" _while="teleop_mode == 1">
-              <Action
-                ID="TeleoperateJointJog"
-                controller_name="servo_controller"
-              />
+              <Action ID="TeleoperateJointJog" controller_name="servo_controller"/>
             </Control>
           </Control>
         </Decorator>
@@ -151,8 +71,8 @@
   <TreeNodesModel>
     <SubTree ID="Request Teleoperation">
       <MetadataFields>
-        <Metadata subcategory="User Input" />
-        <Metadata runnable="false" />
+        <Metadata subcategory="User Input"/>
+        <Metadata runnable="false"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/grinding_sim/objectives/save_tool_pose_yaml.xml b/src/grinding_sim/objectives/save_tool_pose_yaml.xml
index 897df74f..2d17b096 100644
--- a/src/grinding_sim/objectives/save_tool_pose_yaml.xml
+++ b/src/grinding_sim/objectives/save_tool_pose_yaml.xml
@@ -1,38 +1,20 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Save Tool Pose Yaml">
   <!--//////////-->
-  <BehaviorTree
-    ID="Save Tool Pose Yaml"
-    _description=""
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Save Tool Pose Yaml" _description="" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Create a pose at the tool_tip by creating in the tool_tip frame without transforming it -->
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="tool_tip"
-        stamped_pose="{tool_pose_tool_frame}"
-      />
+      <Action ID="CreateStampedPose" reference_frame="tool_tip" stamped_pose="{tool_pose_tool_frame}"/>
       <!-- Transform the pose to the world frame before saving -->
-      <Action
-        ID="TransformPoseFrame"
-        input_pose="{tool_pose_tool_frame}"
-        output_pose="{tool_pose_world_frame}"
-        target_frame_id="world"
-      />
-      <Action
-        ID="SavePoseStampedToYaml"
-        message="{tool_pose_world_frame}"
-        yaml_filename="tool_pose"
-      />
+      <Action ID="TransformPoseFrame" input_pose="{tool_pose_tool_frame}" output_pose="{tool_pose_world_frame}" target_frame_id="world"/>
+      <Action ID="SavePoseStampedToYaml" message="{tool_pose_world_frame}" yaml_filename="tool_pose"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Save Tool Pose Yaml">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Grasping"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/grinding_sim/objectives/take_snapshot.xml b/src/grinding_sim/objectives/take_snapshot.xml
index 09bb0350..bdfd877d 100644
--- a/src/grinding_sim/objectives/take_snapshot.xml
+++ b/src/grinding_sim/objectives/take_snapshot.xml
@@ -1,31 +1,18 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <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"
-  >
+  <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="/scene_camera/points"
-        message_out="{point_cloud}"
-        timeout_sec="15.000000"
-      />
-      <Action
-        ID="SendPointCloudToUI"
-        point_cloud="{point_cloud}"
-        pcd_topic="/pcd_pointcloud_captures"
-      />
+      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
+      <Action ID="GetPointCloud" topic_name="/scene_camera/points" message_out="{point_cloud}" timeout_sec="15.000000"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Take Snapshot">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Perception - 3D Point Cloud" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Perception - 3D Point Cloud"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/grinding_sim/objectives/visualize_poses.xml b/src/grinding_sim/objectives/visualize_poses.xml
index ab399f38..c3e33a89 100644
--- a/src/grinding_sim/objectives/visualize_poses.xml
+++ b/src/grinding_sim/objectives/visualize_poses.xml
@@ -1,69 +1,25 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Visualize Poses">
   <!--//////////-->
-  <BehaviorTree
-    ID="Visualize Poses"
-    _description=""
-    _favorite="true"
-    _subtreeOnly="false"
-    action_poses="action_poses"
-    reference_frame="reference_frame"
-    approach_pose="approach_pose"
-  >
+  <BehaviorTree ID="Visualize Poses" _description="" _favorite="true" _subtreeOnly="false" action_poses="action_poses" reference_frame="reference_frame" approach_pose="approach_pose">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        waypoint_name="Retract"
-      />
-      <SubTree
-        ID="Register Machined Part"
-        _collapsed="false"
-        registered_pose="{registered_pose}"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0" waypoint_name="Retract"/>
+      <SubTree ID="Register Machined Part" _collapsed="false" registered_pose="{registered_pose}"/>
       <!--Publish frame of registered part so we visualize poses in reference to it-->
       <Control ID="Fallback">
         <Decorator ID="Timeout" msec="500">
-          <Action
-            ID="PublishStaticFrame"
-            pose="{registered_pose}"
-            publish_rate="50"
-            child_frame_id="registered_pose"
-          />
+          <Action ID="PublishStaticFrame" pose="{registered_pose}" publish_rate="50" child_frame_id="registered_pose"/>
         </Decorator>
-        <Action ID="AlwaysSuccess" />
+        <Action ID="AlwaysSuccess"/>
       </Control>
-      <Action
-        ID="LoadPoseStampedVectorFromYaml"
-        output="{grinding_poses}"
-        file_path="grinding_poses.yaml"
-      />
+      <Action ID="LoadPoseStampedVectorFromYaml" output="{grinding_poses}" file_path="grinding_poses.yaml"/>
       <!--Use a script with a pose counter so we can give a unique name to each marker (allowing multiple poses to be visualized)-->
-      <Action ID="Script" code="pose_count := 0" />
-      <Decorator
-        ID="ForEach"
-        vector_in="{grinding_poses}"
-        out="{target_pose}"
-        index="{index}"
-      >
+      <Action ID="Script" code="pose_count := 0"/>
+      <Decorator ID="ForEach" vector_in="{grinding_poses}" out="{target_pose}" index="{index}">
         <Control ID="Sequence">
           <!--Visualize and add each pose to the vector-->
-          <Action
-            ID="VisualizePose"
-            marker_name="{pose_count}"
-            pose="{target_pose}"
-            marker_lifetime="0.000000"
-            marker_size="0.100000"
-          />
-          <Action ID="Script" code="pose_count += 1" />
+          <Action ID="VisualizePose" marker_name="{pose_count}" pose="{target_pose}" marker_lifetime="0.000000" marker_size="0.100000"/>
+          <Action ID="Script" code="pose_count += 1"/>
         </Control>
       </Decorator>
     </Control>
@@ -71,8 +27,8 @@
   <TreeNodesModel>
     <SubTree ID="Visualize Poses">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Pose Handling" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Pose Handling"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/grinding_sim/objectives/wait_for_trajectory_approval_if_user_available.xml b/src/grinding_sim/objectives/wait_for_trajectory_approval_if_user_available.xml
index 2f6a4131..785bd54d 100644
--- a/src/grinding_sim/objectives/wait_for_trajectory_approval_if_user_available.xml
+++ b/src/grinding_sim/objectives/wait_for_trajectory_approval_if_user_available.xml
@@ -1,24 +1,17 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Wait for Trajectory Approval if User Available"
->
-  <BehaviorTree
-    ID="Wait for Trajectory Approval if User Available"
-    _description="Preview a shared pointer to an MTC Solution object via an input data port if a user is available."
-    _subtreeOnly="true"
-  >
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Wait for Trajectory Approval if User Available">
+  <BehaviorTree ID="Wait for Trajectory Approval if User Available" _description="Preview a shared pointer to an MTC Solution object via an input data port if a user is available." _subtreeOnly="true">
     <Control ID="Fallback" name="wait_for_approval_if_user_available">
       <Decorator ID="Inverter">
-        <Action ID="IsUserAvailable" />
+        <Action ID="IsUserAvailable"/>
       </Decorator>
-      <Action ID="WaitForUserTrajectoryApproval" solution="{solution}" />
+      <Action ID="WaitForUserTrajectoryApproval" solution="{solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Wait for Trajectory Approval if User Available">
       <MetadataFields>
-        <Metadata subcategory="User Input" />
+        <Metadata subcategory="User Input"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/grinding_sim/package.xml b/src/grinding_sim/package.xml
index 7033f70a..e08e18d4 100644
--- a/src/grinding_sim/package.xml
+++ b/src/grinding_sim/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>grinding_sim</name>
   <version>7.0.0</version>
diff --git a/src/hangar_sim/description/front_left_wheel_link.xml b/src/hangar_sim/description/front_left_wheel_link.xml
index f06fcf85..9384dddf 100644
--- a/src/hangar_sim/description/front_left_wheel_link.xml
+++ b/src/hangar_sim/description/front_left_wheel_link.xml
@@ -1,357 +1,77 @@
+<?xml version='1.0' encoding='utf-8'?>
 <body name="front_left_wheel_intermediate_link" pos="0 0 0">
-  <inertial
-    pos="0 0 0"
-    quat="0.707107 0 0 0.707107"
-    mass="1.0"
-    diaginertia="0.0524193 0.0303095 0.0303095"
-  />
-  <joint name="front_left_wheel" pos="0 0 0" axis="0 1 0" />
+  <inertial pos="0 0 0" quat="0.707107 0 0 0.707107" mass="1.0" diaginertia="0.0524193 0.0303095 0.0303095"/>
+  <joint name="front_left_wheel" pos="0 0 0" axis="0 1 0"/>
 
-  <body
-    name="front_left_roller_0_link"
-    pos="0.0438515969889343 0.0 -0.01175000000000001"
-  >
-    <joint
-      name="front_left_slipping_0_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.006296806022131396 0.0435 -0.02350000000000002"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_left_roller_0_link" pos="0.0438515969889343 0.0 -0.01175000000000001">
+    <joint name="front_left_slipping_0_joint" type="hinge" pos="0 0 0" axis="-0.006296806022131396 0.0435 -0.02350000000000002" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_left_roller_1_link"
-    pos="0.04385159698893431 0.0 0.011749999999999998"
-  >
-    <joint
-      name="front_left_slipping_1_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.006296806022131382 0.0435 -0.023499999999999997"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_left_roller_1_link" pos="0.04385159698893431 0.0 0.011749999999999998">
+    <joint name="front_left_slipping_1_joint" type="hinge" pos="0 0 0" axis="0.006296806022131382 0.0435 -0.023499999999999997" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_left_roller_2_link"
-    pos="0.03210159698893431 0.0 0.032101596988934306"
-  >
-    <joint
-      name="front_left_slipping_2_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.01720319397786861 0.0435 -0.017203193977868615"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_left_roller_2_link" pos="0.03210159698893431 0.0 0.032101596988934306">
+    <joint name="front_left_slipping_2_joint" type="hinge" pos="0 0 0" axis="0.01720319397786861 0.0435 -0.017203193977868615" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_left_roller_3_link"
-    pos="0.011750000000000005 0.0 0.0438515969889343"
-  >
-    <joint
-      name="front_left_slipping_3_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.023500000000000004 0.0435 -0.006296806022131389"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_left_roller_3_link" pos="0.011750000000000005 0.0 0.0438515969889343">
+    <joint name="front_left_slipping_3_joint" type="hinge" pos="0 0 0" axis="0.023500000000000004 0.0435 -0.006296806022131389" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_left_roller_4_link"
-    pos="-0.011749999999999993 0.0 0.04385159698893431"
-  >
-    <joint
-      name="front_left_slipping_4_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.023499999999999993 0.0435 0.006296806022131382"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_left_roller_4_link" pos="-0.011749999999999993 0.0 0.04385159698893431">
+    <joint name="front_left_slipping_4_joint" type="hinge" pos="0 0 0" axis="0.023499999999999993 0.0435 0.006296806022131382" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_left_roller_5_link"
-    pos="-0.0321015969889343 0.0 0.03210159698893432"
-  >
-    <joint
-      name="front_left_slipping_5_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.01720319397786862 0.0435 0.0172031939778686"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_left_roller_5_link" pos="-0.0321015969889343 0.0 0.03210159698893432">
+    <joint name="front_left_slipping_5_joint" type="hinge" pos="0 0 0" axis="0.01720319397786862 0.0435 0.0172031939778686" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_left_roller_6_link"
-    pos="-0.0438515969889343 0.0 0.01175000000000001"
-  >
-    <joint
-      name="front_left_slipping_6_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.006296806022131389 0.0435 0.02350000000000001"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_left_roller_6_link" pos="-0.0438515969889343 0.0 0.01175000000000001">
+    <joint name="front_left_slipping_6_joint" type="hinge" pos="0 0 0" axis="0.006296806022131389 0.0435 0.02350000000000001" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_left_roller_7_link"
-    pos="-0.043851596988934316 0.0 -0.01174999999999999"
-  >
-    <joint
-      name="front_left_slipping_7_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.006296806022131375 0.0435 0.023499999999999993"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_left_roller_7_link" pos="-0.043851596988934316 0.0 -0.01174999999999999">
+    <joint name="front_left_slipping_7_joint" type="hinge" pos="0 0 0" axis="-0.006296806022131375 0.0435 0.023499999999999993" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_left_roller_8_link"
-    pos="-0.03210159698893432 0.0 -0.03210159698893429"
-  >
-    <joint
-      name="front_left_slipping_8_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.017203193977868604 0.0435 0.017203193977868618"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_left_roller_8_link" pos="-0.03210159698893432 0.0 -0.03210159698893429">
+    <joint name="front_left_slipping_8_joint" type="hinge" pos="0 0 0" axis="-0.017203193977868604 0.0435 0.017203193977868618" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_left_roller_9_link"
-    pos="-0.011750000000000016 0.0 -0.0438515969889343"
-  >
-    <joint
-      name="front_left_slipping_9_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.023500000000000014 0.0435 0.006296806022131396"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_left_roller_9_link" pos="-0.011750000000000016 0.0 -0.0438515969889343">
+    <joint name="front_left_slipping_9_joint" type="hinge" pos="0 0 0" axis="-0.023500000000000014 0.0435 0.006296806022131396" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_left_roller_10_link"
-    pos="0.011749999999999981 0.0 -0.043851596988934316"
-  >
-    <joint
-      name="front_left_slipping_10_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.023499999999999976 0.0435 -0.006296806022131368"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_left_roller_10_link" pos="0.011749999999999981 0.0 -0.043851596988934316">
+    <joint name="front_left_slipping_10_joint" type="hinge" pos="0 0 0" axis="-0.023499999999999976 0.0435 -0.006296806022131368" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_left_roller_11_link"
-    pos="0.032101596988934285 0.0 -0.032101596988934326"
-  >
-    <joint
-      name="front_left_slipping_11_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.017203193977868635 0.0435 -0.01720319397786861"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_left_roller_11_link" pos="0.032101596988934285 0.0 -0.032101596988934326">
+    <joint name="front_left_slipping_11_joint" type="hinge" pos="0 0 0" axis="-0.017203193977868635 0.0435 -0.01720319397786861" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 </body>
diff --git a/src/hangar_sim/description/front_right_wheel_link.xml b/src/hangar_sim/description/front_right_wheel_link.xml
index 38c6acce..38316649 100644
--- a/src/hangar_sim/description/front_right_wheel_link.xml
+++ b/src/hangar_sim/description/front_right_wheel_link.xml
@@ -1,357 +1,77 @@
+<?xml version='1.0' encoding='utf-8'?>
 <body name="front_right_wheel_intermediate_link" pos="0 0 0">
-  <inertial
-    pos="0 0 0"
-    quat="0.707107 0 0 0.707107"
-    mass="1.0"
-    diaginertia="0.0524193 0.0303095 0.0303095"
-  />
-  <joint name="front_right_wheel" pos="0 0 0" axis="0 1 0" />
+  <inertial pos="0 0 0" quat="0.707107 0 0 0.707107" mass="1.0" diaginertia="0.0524193 0.0303095 0.0303095"/>
+  <joint name="front_right_wheel" pos="0 0 0" axis="0 1 0"/>
 
-  <body
-    name="front_right_roller_0_link"
-    pos="0.04385159698893431 0.0 0.011749999999999998"
-  >
-    <joint
-      name="front_right_slipping_0_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.006296806022131382 0.0435 0.023499999999999997"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_right_roller_0_link" pos="0.04385159698893431 0.0 0.011749999999999998">
+    <joint name="front_right_slipping_0_joint" type="hinge" pos="0 0 0" axis="-0.006296806022131382 0.0435 0.023499999999999997" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_right_roller_1_link"
-    pos="0.03210159698893431 0.0 0.032101596988934306"
-  >
-    <joint
-      name="front_right_slipping_1_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.01720319397786861 0.0435 0.017203193977868615"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_right_roller_1_link" pos="0.03210159698893431 0.0 0.032101596988934306">
+    <joint name="front_right_slipping_1_joint" type="hinge" pos="0 0 0" axis="-0.01720319397786861 0.0435 0.017203193977868615" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_right_roller_2_link"
-    pos="0.011750000000000005 0.0 0.0438515969889343"
-  >
-    <joint
-      name="front_right_slipping_2_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.023500000000000004 0.0435 0.006296806022131389"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_right_roller_2_link" pos="0.011750000000000005 0.0 0.0438515969889343">
+    <joint name="front_right_slipping_2_joint" type="hinge" pos="0 0 0" axis="-0.023500000000000004 0.0435 0.006296806022131389" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_right_roller_3_link"
-    pos="-0.011749999999999993 0.0 0.04385159698893431"
-  >
-    <joint
-      name="front_right_slipping_3_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.023499999999999993 0.0435 -0.006296806022131382"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_right_roller_3_link" pos="-0.011749999999999993 0.0 0.04385159698893431">
+    <joint name="front_right_slipping_3_joint" type="hinge" pos="0 0 0" axis="-0.023499999999999993 0.0435 -0.006296806022131382" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_right_roller_4_link"
-    pos="-0.0321015969889343 0.0 0.03210159698893432"
-  >
-    <joint
-      name="front_right_slipping_4_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.01720319397786862 0.0435 -0.0172031939778686"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_right_roller_4_link" pos="-0.0321015969889343 0.0 0.03210159698893432">
+    <joint name="front_right_slipping_4_joint" type="hinge" pos="0 0 0" axis="-0.01720319397786862 0.0435 -0.0172031939778686" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_right_roller_5_link"
-    pos="-0.0438515969889343 0.0 0.011750000000000012"
-  >
-    <joint
-      name="front_right_slipping_5_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.006296806022131389 0.0435 -0.02350000000000001"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_right_roller_5_link" pos="-0.0438515969889343 0.0 0.011750000000000012">
+    <joint name="front_right_slipping_5_joint" type="hinge" pos="0 0 0" axis="-0.006296806022131389 0.0435 -0.02350000000000001" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_right_roller_6_link"
-    pos="-0.043851596988934316 0.0 -0.011749999999999991"
-  >
-    <joint
-      name="front_right_slipping_6_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.006296806022131375 0.0435 -0.023499999999999993"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_right_roller_6_link" pos="-0.043851596988934316 0.0 -0.011749999999999991">
+    <joint name="front_right_slipping_6_joint" type="hinge" pos="0 0 0" axis="0.006296806022131375 0.0435 -0.023499999999999993" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_right_roller_7_link"
-    pos="-0.03210159698893432 0.0 -0.03210159698893429"
-  >
-    <joint
-      name="front_right_slipping_7_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.017203193977868604 0.0435 -0.017203193977868618"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_right_roller_7_link" pos="-0.03210159698893432 0.0 -0.03210159698893429">
+    <joint name="front_right_slipping_7_joint" type="hinge" pos="0 0 0" axis="0.017203193977868604 0.0435 -0.017203193977868618" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_right_roller_8_link"
-    pos="-0.011750000000000014 0.0 -0.0438515969889343"
-  >
-    <joint
-      name="front_right_slipping_8_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.023500000000000014 0.0435 -0.006296806022131396"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_right_roller_8_link" pos="-0.011750000000000014 0.0 -0.0438515969889343">
+    <joint name="front_right_slipping_8_joint" type="hinge" pos="0 0 0" axis="0.023500000000000014 0.0435 -0.006296806022131396" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_right_roller_9_link"
-    pos="0.01174999999999998 0.0 -0.043851596988934316"
-  >
-    <joint
-      name="front_right_slipping_9_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.023499999999999976 0.0435 0.006296806022131368"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_right_roller_9_link" pos="0.01174999999999998 0.0 -0.043851596988934316">
+    <joint name="front_right_slipping_9_joint" type="hinge" pos="0 0 0" axis="0.023499999999999976 0.0435 0.006296806022131368" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_right_roller_10_link"
-    pos="0.032101596988934285 0.0 -0.032101596988934326"
-  >
-    <joint
-      name="front_right_slipping_10_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.017203193977868635 0.0435 0.01720319397786861"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_right_roller_10_link" pos="0.032101596988934285 0.0 -0.032101596988934326">
+    <joint name="front_right_slipping_10_joint" type="hinge" pos="0 0 0" axis="0.017203193977868635 0.0435 0.01720319397786861" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="front_right_roller_11_link"
-    pos="0.0438515969889343 0.0 -0.01175000000000001"
-  >
-    <joint
-      name="front_right_slipping_11_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.006296806022131396 0.0435 0.02350000000000002"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="front_right_roller_11_link" pos="0.0438515969889343 0.0 -0.01175000000000001">
+    <joint name="front_right_slipping_11_joint" type="hinge" pos="0 0 0" axis="0.006296806022131396 0.0435 0.02350000000000002" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 </body>
diff --git a/src/hangar_sim/description/hangar.xml b/src/hangar_sim/description/hangar.xml
index 9d67fddc..3c498bef 100644
--- a/src/hangar_sim/description/hangar.xml
+++ b/src/hangar_sim/description/hangar.xml
@@ -1,3514 +1,955 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco>
-  <compiler angle="radian" meshdir="assets" autolimits="true" />
+  <compiler angle="radian" meshdir="assets" autolimits="true"/>
   <visual>
-    <headlight diffuse="0.5 0.5 0.5" ambient="0.6 0.6 0.6" specular="0 0 0" />
-    <rgba haze="0.15 0.25 0.35 1" />
-    <global azimuth="120" elevation="-20" />
+    <headlight diffuse="0.5 0.5 0.5" ambient="0.6 0.6 0.6" specular="0 0 0"/>
+    <rgba haze="0.15 0.25 0.35 1"/>
+    <global azimuth="120" elevation="-20"/>
   </visual>
   <asset>
-    <texture
-      type="skybox"
-      file="assets/space.png"
-      gridsize="3 4"
-      gridlayout=".U..LFRB.D.."
-    />
+    <texture type="skybox" file="assets/space.png" gridsize="3 4" gridlayout=".U..LFRB.D.."/>
   </asset>
   <worldbody>
-    <body
-      name="SM_WallSide4"
-      pos="-25.105867385864258 32.0594482421875 10.189191818237305"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSide4 geom"
-        mesh="SM_WallSide4"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSide16"
-      pos="25.105857849121094 54.55787658691406 10.189191818237305"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSide16 geom"
-        mesh="SM_WallSide16"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_Roof4"
-      pos="-12.036163330078125 24.558650970458984 18.083402633666992"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Roof4 geom"
-        mesh="SM_Roof4"
-        material="glass"
-      />
-    </body>
-    <body
-      name="SM_Roof16_360"
-      pos="12.03618335723877 2.0586605072021484 18.08340835571289"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Roof16_360 geom"
-        mesh="SM_Roof16_360"
-        material="glass"
-      />
-    </body>
-    <body
-      name="SM_Floor_376"
-      pos="0.0 25.80866050720215 -0.5"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Floor_376 geom"
-        mesh="SM_Floor_376"
-        material="floor"
-      />
-    </body>
-    <body
-      name="SM_WallFront_382"
-      pos="-0.001163438311778009 -1.5790290832519531 14.412371635437012"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallFront_382 geom"
-        mesh="SM_WallFront_382"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallBack_396"
-      pos="-0.0011624015169218183 58.21354293823242 11.401654243469238"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallBack_396 geom"
-        mesh="SM_WallBack_396"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_DoorSlide_399"
-      pos="-0.026868805289268494 57.99137496948242 6.278013706207275"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DoorSlide_399 geom"
-        mesh="SM_DoorSlide_399"
-        material="glass"
-      />
-    </body>
-    <body
-      name="SM_WallSupport"
-      pos="-18.47951889038086 50.80866622924805 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport geom"
-        mesh="SM_WallSupport"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSupport2"
-      pos="-18.47951889038086 43.30866622924805 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport2 geom"
-        mesh="SM_WallSupport2"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSupport3"
-      pos="-18.47951889038086 35.80866622924805 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport3 geom"
-        mesh="SM_WallSupport3"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSupport4"
-      pos="-18.47951889038086 28.30866813659668 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport4 geom"
-        mesh="SM_WallSupport4"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSupport5"
-      pos="-18.47951889038086 20.80866813659668 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport5 geom"
-        mesh="SM_WallSupport5"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSupport6"
-      pos="-18.47951889038086 13.30866813659668 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport6 geom"
-        mesh="SM_WallSupport6"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSupport7"
-      pos="-18.47951889038086 5.80866813659668 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport7 geom"
-        mesh="SM_WallSupport7"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSupport8"
-      pos="18.479324340820312 5.80864143371582 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport8 geom"
-        mesh="SM_WallSupport8"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSupport9"
-      pos="18.479366302490234 13.308650016784668 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport9 geom"
-        mesh="SM_WallSupport9"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSupport10"
-      pos="18.479389190673828 20.80864715576172 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport10 geom"
-        mesh="SM_WallSupport10"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSupport11"
-      pos="18.479413986206055 28.308656692504883 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport11 geom"
-        mesh="SM_WallSupport11"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSupport12"
-      pos="18.47945785522461 35.80866622924805 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport12 geom"
-        mesh="SM_WallSupport12"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSupport13"
-      pos="18.479488372802734 43.30866622924805 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport13 geom"
-        mesh="SM_WallSupport13"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSupport14"
-      pos="18.47951889038086 50.80866622924805 12.53268051147461"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSupport14 geom"
-        mesh="SM_WallSupport14"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_Room_426"
-      pos="17.14999771118164 48.308658599853516 2.6000020503997803"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Room_426 geom"
-        mesh="SM_Room_426"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_DoorSingle_430"
-      pos="-8.858838081359863 58.2630500793457 1.0016013383865356"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DoorSingle_430 geom"
-        mesh="SM_DoorSingle_430"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_DoorSingle2"
-      pos="-11.9847412109375 48.04981994628906 1.0025235414505005"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DoorSingle2 geom"
-        mesh="SM_DoorSingle2"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_AccessStand_441"
-      pos="-19.30655288696289 16.40500831604004 0.8803966641426086"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_AccessStand_441 geom"
-        mesh="SM_AccessStand_441"
-        material="yellow metal"
-      />
-    </body>
-    <body
-      name="SM_Lamps6"
-      pos="13.000012397766113 13.308566093444824 14.159097671508789"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Lamps6 geom"
-        mesh="SM_Lamps6"
-        material="light"
-      />
-    </body>
-    <body
-      name="SM_TrashBin_22"
-      pos="24.357282638549805 29.708660125732422 0.6565067172050476"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_TrashBin_22 geom"
-        mesh="SM_TrashBin_22"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_Cube_004"
-      pos="-11.981542587280273 6.327070713043213 0.5064430832862854"
-      quat="0.9604685306549072 0.0 0.0 -0.27838853001594543"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube_004 geom"
-        mesh="collision_Cube_004"
-      />
-      <body
-        name="SM_Ladder_31"
-        pos="0.0027790069580078125 0.03064107894897461 0.13250821828842163"
-        quat="1.0 0.0 0.0 0.0"
-      >
-        <geom
-          class="visual"
-          type="mesh"
-          name="SM_Ladder_31 geom"
-          mesh="SM_Ladder_31"
-          material="metal_hangar"
-        />
+    <body name="SM_WallSide4" pos="-25.105867385864258 32.0594482421875 10.189191818237305" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSide4 geom" mesh="SM_WallSide4" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSide16" pos="25.105857849121094 54.55787658691406 10.189191818237305" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSide16 geom" mesh="SM_WallSide16" material="metal_hangar"/>
+    </body>
+    <body name="SM_Roof4" pos="-12.036163330078125 24.558650970458984 18.083402633666992" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Roof4 geom" mesh="SM_Roof4" material="glass"/>
+    </body>
+    <body name="SM_Roof16_360" pos="12.03618335723877 2.0586605072021484 18.08340835571289" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Roof16_360 geom" mesh="SM_Roof16_360" material="glass"/>
+    </body>
+    <body name="SM_Floor_376" pos="0.0 25.80866050720215 -0.5" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Floor_376 geom" mesh="SM_Floor_376" material="floor"/>
+    </body>
+    <body name="SM_WallFront_382" pos="-0.001163438311778009 -1.5790290832519531 14.412371635437012" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallFront_382 geom" mesh="SM_WallFront_382" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallBack_396" pos="-0.0011624015169218183 58.21354293823242 11.401654243469238" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallBack_396 geom" mesh="SM_WallBack_396" material="metal_hangar"/>
+    </body>
+    <body name="SM_DoorSlide_399" pos="-0.026868805289268494 57.99137496948242 6.278013706207275" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DoorSlide_399 geom" mesh="SM_DoorSlide_399" material="glass"/>
+    </body>
+    <body name="SM_WallSupport" pos="-18.47951889038086 50.80866622924805 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport geom" mesh="SM_WallSupport" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSupport2" pos="-18.47951889038086 43.30866622924805 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport2 geom" mesh="SM_WallSupport2" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSupport3" pos="-18.47951889038086 35.80866622924805 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport3 geom" mesh="SM_WallSupport3" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSupport4" pos="-18.47951889038086 28.30866813659668 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport4 geom" mesh="SM_WallSupport4" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSupport5" pos="-18.47951889038086 20.80866813659668 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport5 geom" mesh="SM_WallSupport5" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSupport6" pos="-18.47951889038086 13.30866813659668 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport6 geom" mesh="SM_WallSupport6" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSupport7" pos="-18.47951889038086 5.80866813659668 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport7 geom" mesh="SM_WallSupport7" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSupport8" pos="18.479324340820312 5.80864143371582 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport8 geom" mesh="SM_WallSupport8" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSupport9" pos="18.479366302490234 13.308650016784668 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport9 geom" mesh="SM_WallSupport9" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSupport10" pos="18.479389190673828 20.80864715576172 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport10 geom" mesh="SM_WallSupport10" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSupport11" pos="18.479413986206055 28.308656692504883 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport11 geom" mesh="SM_WallSupport11" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSupport12" pos="18.47945785522461 35.80866622924805 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport12 geom" mesh="SM_WallSupport12" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSupport13" pos="18.479488372802734 43.30866622924805 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport13 geom" mesh="SM_WallSupport13" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSupport14" pos="18.47951889038086 50.80866622924805 12.53268051147461" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSupport14 geom" mesh="SM_WallSupport14" material="metal_hangar"/>
+    </body>
+    <body name="SM_Room_426" pos="17.14999771118164 48.308658599853516 2.6000020503997803" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Room_426 geom" mesh="SM_Room_426" material="walls"/>
+    </body>
+    <body name="SM_DoorSingle_430" pos="-8.858838081359863 58.2630500793457 1.0016013383865356" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DoorSingle_430 geom" mesh="SM_DoorSingle_430" material="metal_hangar"/>
+    </body>
+    <body name="SM_DoorSingle2" pos="-11.9847412109375 48.04981994628906 1.0025235414505005" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DoorSingle2 geom" mesh="SM_DoorSingle2" material="metal_hangar"/>
+    </body>
+    <body name="SM_AccessStand_441" pos="-19.30655288696289 16.40500831604004 0.8803966641426086" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_AccessStand_441 geom" mesh="SM_AccessStand_441" material="yellow metal"/>
+    </body>
+    <body name="SM_Lamps6" pos="13.000012397766113 13.308566093444824 14.159097671508789" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Lamps6 geom" mesh="SM_Lamps6" material="light"/>
+    </body>
+    <body name="SM_TrashBin_22" pos="24.357282638549805 29.708660125732422 0.6565067172050476" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_TrashBin_22 geom" mesh="SM_TrashBin_22" material="metal_hangar"/>
+    </body>
+    <body name="collision_Cube_004" pos="-11.981542587280273 6.327070713043213 0.5064430832862854" quat="0.9604685306549072 0.0 0.0 -0.27838853001594543">
+      <geom class="collision" type="mesh" name="collision_Cube_004 geom" mesh="collision_Cube_004"/>
+      <body name="SM_Ladder_31" pos="0.0027790069580078125 0.03064107894897461 0.13250821828842163" quat="1.0 0.0 0.0 0.0">
+        <geom class="visual" type="mesh" name="SM_Ladder_31 geom" mesh="SM_Ladder_31" material="metal_hangar"/>
       </body>
     </body>
 
-    <body
-      name="SM_Shelf_A2"
-      pos="23.998563766479492 8.312459945678711 1.1799222230911255"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Shelf_A2 geom"
-        mesh="SM_Shelf_A2"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_Shelf_A3"
-      pos="-24.00143814086914 15.81245231628418 1.1799222230911255"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Shelf_A3 geom"
-        mesh="SM_Shelf_A3"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_Shelf_B_39"
-      pos="-23.850627899169922 10.819804191589355 0.9544692635536194"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Shelf_B_39 geom"
-        mesh="SM_Shelf_B_39"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_Shelf_B2"
-      pos="24.149372100830078 11.319804191589355 0.9544692635536194"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Shelf_B2 geom"
-        mesh="SM_Shelf_B2"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_Shelf_B3"
-      pos="24.149372100830078 15.819796562194824 0.9544692635536194"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Shelf_B3 geom"
-        mesh="SM_Shelf_B3"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_Box_A_44"
-      pos="-20.89999771118164 54.308658599853516 5.436284065246582"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_A_44 geom"
-        mesh="SM_Box_A_44"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_A2"
-      pos="-18.699996948242188 54.308658599853516 5.436284065246582"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_A2 geom"
-        mesh="SM_Box_A2"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_A3"
-      pos="-16.499998092651367 54.308658599853516 5.436284065246582"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_A3 geom"
-        mesh="SM_Box_A3"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_A4"
-      pos="-20.89999771118164 51.60866165161133 5.436284065246582"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_A4 geom"
-        mesh="SM_Box_A4"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_A5"
-      pos="-18.699996948242188 51.60866165161133 5.436284065246582"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_A5 geom"
-        mesh="SM_Box_A5"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_A6"
-      pos="-16.499998092651367 51.60866165161133 5.436284065246582"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_A6 geom"
-        mesh="SM_Box_A6"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_B_54"
-      pos="-14.19999885559082 53.70866012573242 5.239261150360107"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_B_54 geom"
-        mesh="SM_Box_B_54"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_B2"
-      pos="-13.299997329711914 53.70866012573242 5.239261150360107"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_B2 geom"
-        mesh="SM_Box_B2"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_B3"
-      pos="-12.399999618530273 53.70866012573242 5.239261150360107"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_B3 geom"
-        mesh="SM_Box_B3"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_B4"
-      pos="-11.5 53.70866012573242 5.239261150360107"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_B4 geom"
-        mesh="SM_Box_B4"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_B5"
-      pos="-10.59999942779541 53.70866012573242 5.239261150360107"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_B5 geom"
-        mesh="SM_Box_B5"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_B6"
-      pos="-9.69999885559082 53.70866012573242 5.239261150360107"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_B6 geom"
-        mesh="SM_Box_B6"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_B7"
-      pos="-8.800000190734863 53.70866012573242 5.239261150360107"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_B7 geom"
-        mesh="SM_Box_B7"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_C_63"
-      pos="-23.299999237060547 46.808658599853516 4.965346336364746"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_C_63 geom"
-        mesh="SM_Box_C_63"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_C2"
-      pos="-21.999998092651367 46.808658599853516 4.965346336364746"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_C2 geom"
-        mesh="SM_Box_C2"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_C3"
-      pos="-20.699996948242188 46.808658599853516 4.965346336364746"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_C3 geom"
-        mesh="SM_Box_C3"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_C4_68"
-      pos="23.500001907348633 53.15865707397461 4.965346336364746"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_C4_68 geom"
-        mesh="SM_Box_C4_68"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_C5"
-      pos="22.19999885559082 53.15865707397461 4.965346336364746"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_C5 geom"
-        mesh="SM_Box_C5"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_C6"
-      pos="20.900001525878906 53.15865707397461 4.965346336364746"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_C6 geom"
-        mesh="SM_Box_C6"
-        material="box"
-      />
-    </body>
-    <body
-      name="collision_SM_Box_A7_73"
-      pos="-22.699996948242188 0.2086658477783203 1.936284065246582"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Box_A7_73 geom"
-        mesh="collision_SM_Box_A7_73"
-      />
-    </body>
-    <body
-      name="collision_SM_Box_A8"
-      pos="-22.699996948242188 2.908662796020508 1.936284065246582"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Box_A8 geom"
-        mesh="collision_SM_Box_A8"
-      />
-    </body>
-    <body
-      name="collision_SM_Box_C7_77"
-      pos="-20.899995803833008 1.1086597442626953 1.3653459548950195"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Box_C7_77 geom"
-        mesh="collision_SM_Box_C7_77"
-      />
-    </body>
-    <body
-      name="SM_Room2_3"
-      pos="-17.25 48.308658599853516 2.600001335144043"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Room2_3 geom"
-        mesh="SM_Room2_3"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_Pillar_10"
-      pos="-9.499999046325684 42.808658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Pillar_10 geom"
-        mesh="SM_Pillar_10"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_Pillar2"
-      pos="-9.499999046325684 46.308658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Pillar2 geom"
-        mesh="SM_Pillar2"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_Pillar3"
-      pos="-9.499999046325684 50.308658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Pillar3 geom"
-        mesh="SM_Pillar3"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_Pillar4"
-      pos="-9.499999046325684 53.808658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Pillar4 geom"
-        mesh="SM_Pillar4"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_Pillar5"
-      pos="9.000000953674316 42.808658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Pillar5 geom"
-        mesh="SM_Pillar5"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_Pillar6"
-      pos="9.000000953674316 46.308658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Pillar6 geom"
-        mesh="SM_Pillar6"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_Pillar7"
-      pos="9.000000953674316 50.308658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Pillar7 geom"
-        mesh="SM_Pillar7"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_Pillar8"
-      pos="9.000000953674316 53.808658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Pillar8 geom"
-        mesh="SM_Pillar8"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_DoorSingle3"
-      pos="11.854381561279297 48.067501068115234 1.0016013383865356"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DoorSingle3 geom"
-        mesh="SM_DoorSingle3"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_DoorDouble_26"
-      pos="25.2814998626709 24.80866050720215 1.0649999380111694"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DoorDouble_26 geom"
-        mesh="SM_DoorDouble_26"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_DoorDouble2"
-      pos="-25.000579833984375 24.30866050720215 1.0649999380111694"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DoorDouble2 geom"
-        mesh="SM_DoorDouble2"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_DoorSingle4"
-      pos="21.258838653564453 -1.6457252502441406 1.0016013383865356"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DoorSingle4 geom"
-        mesh="SM_DoorSingle4"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_Stairs_32"
-      pos="17.748929977416992 39.79243087768555 1.639772653579712"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Stairs_32 geom"
-        mesh="SM_Stairs_32"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_Stairs2"
-      pos="-17.748912811279297 39.792415618896484 1.6397724151611328"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Stairs2 geom"
-        mesh="SM_Stairs2"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_StairsRailing_Inner_2"
-      pos="18.00760841369629 40.89141845703125 2.7805166244506836"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_StairsRailing_Inner_2 geom"
-        mesh="SM_StairsRailing_Inner_2"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_StairsRailing_Outer_7"
-      pos="18.999317169189453 38.7586784362793 2.959934949874878"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_StairsRailing_Outer_7 geom"
-        mesh="SM_StairsRailing_Outer_7"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_StairsRailing_Top_10"
-      pos="14.234500885009766 41.46199035644531 4.4237823486328125"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_StairsRailing_Top_10 geom"
-        mesh="SM_StairsRailing_Top_10"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_StairsRailing_Outer2"
-      pos="-18.999317169189453 38.75865173339844 2.959934949874878"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_StairsRailing_Outer2 geom"
-        mesh="SM_StairsRailing_Outer2"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_StairsRailing_Inner2"
-      pos="-18.057634353637695 40.89144515991211 2.7805166244506836"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_StairsRailing_Inner2 geom"
-        mesh="SM_StairsRailing_Inner2"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_StairsRailing_Top2_15"
-      pos="-14.334492683410645 41.45530319213867 4.4237823486328125"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_StairsRailing_Top2_15 geom"
-        mesh="SM_StairsRailing_Top2_15"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_DoorSingle5"
-      pos="-21.458837509155273 41.26304626464844 1.0016013383865356"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DoorSingle5 geom"
-        mesh="SM_DoorSingle5"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_DoorSingle6"
-      pos="20.941164016723633 41.26304626464844 1.0016013383865356"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DoorSingle6 geom"
-        mesh="SM_DoorSingle6"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H_2"
-      pos="0.0 28.20865821838379 19.900009155273438"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H_2 geom"
-        mesh="SM_WallRailing_H_2"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H2_5"
-      pos="2.500000238418579 28.20865821838379 19.400009155273438"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H2_5 geom"
-        mesh="SM_WallRailing_H2_5"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H3"
-      pos="5.0 28.20865821838379 19.000009536743164"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H3 geom"
-        mesh="SM_WallRailing_H3"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H4"
-      pos="7.499999523162842 28.20865821838379 18.500009536743164"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H4 geom"
-        mesh="SM_WallRailing_H4"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H5"
-      pos="9.999999046325684 28.20865821838379 18.10000991821289"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H5 geom"
-        mesh="SM_WallRailing_H5"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H6"
-      pos="12.499999046325684 28.20865821838379 17.60000991821289"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H6 geom"
-        mesh="SM_WallRailing_H6"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H7"
-      pos="14.999999046325684 28.20865821838379 17.200008392333984"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H7 geom"
-        mesh="SM_WallRailing_H7"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H8"
-      pos="17.5 28.20865821838379 16.80000877380371"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H8 geom"
-        mesh="SM_WallRailing_H8"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H9"
-      pos="20.0 28.20865821838379 16.30000877380371"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H9 geom"
-        mesh="SM_WallRailing_H9"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H10"
-      pos="22.500001907348633 28.20865821838379 15.900009155273438"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H10 geom"
-        mesh="SM_WallRailing_H10"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H11"
-      pos="-2.499999761581421 28.20865821838379 19.400009155273438"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H11 geom"
-        mesh="SM_WallRailing_H11"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H12"
-      pos="-5.0 28.20865821838379 19.000009536743164"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H12 geom"
-        mesh="SM_WallRailing_H12"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H13"
-      pos="-7.500000476837158 28.20865821838379 18.500009536743164"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H13 geom"
-        mesh="SM_WallRailing_H13"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H14"
-      pos="-10.000000953674316 28.20865821838379 18.10000991821289"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H14 geom"
-        mesh="SM_WallRailing_H14"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H15"
-      pos="-12.500000953674316 28.20865821838379 17.60000991821289"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H15 geom"
-        mesh="SM_WallRailing_H15"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H16"
-      pos="-15.000000953674316 28.20865821838379 17.200008392333984"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H16 geom"
-        mesh="SM_WallRailing_H16"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H17"
-      pos="-17.5 28.20865821838379 16.80000877380371"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H17 geom"
-        mesh="SM_WallRailing_H17"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H18"
-      pos="-20.0 28.20865821838379 16.400009155273438"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H18 geom"
-        mesh="SM_WallRailing_H18"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallRailing_H19"
-      pos="-22.499998092651367 28.20865821838379 15.900009155273438"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallRailing_H19 geom"
-        mesh="SM_WallRailing_H19"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_DoorDouble3"
-      pos="24.936420440673828 24.30866050720215 1.0649999380111694"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DoorDouble3 geom"
-        mesh="SM_DoorDouble3"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_AccessStand2"
-      pos="11.576964378356934 9.308902740478516 0.8804012537002563"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_AccessStand2 geom"
-        mesh="SM_AccessStand2"
-        material="yellow metal"
-      />
-    </body>
-    <body
-      name="SM_Ladder2"
-      pos="-23.856979370117188 32.41215515136719 0.9320219159126282"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Ladder2 geom"
-        mesh="SM_Ladder2"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_Ladder3"
-      pos="-23.48493003845215 31.00689125061035 0.9328975081443787"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Ladder3 geom"
-        mesh="SM_Ladder3"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_TrashBin2"
-      pos="24.357282638549805 30.5086612701416 0.6565067172050476"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_TrashBin2 geom"
-        mesh="SM_TrashBin2"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_Box_C8"
-      pos="20.941354751586914 56.158653259277344 1.3647199869155884"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom class="visual" type="mesh" name="SM_Box_C8 geom" mesh="SM_Box_C8" />
-    </body>
-    <body
-      name="SM_Box_C9"
-      pos="20.641357421875 57.458656311035156 1.26472008228302"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom class="visual" type="mesh" name="SM_Box_C9 geom" mesh="SM_Box_C9" />
-    </body>
-    <body
-      name="SM_Box_C10"
-      pos="-20.699996948242188 56.70866012573242 1.3653464317321777"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_C10 geom"
-        mesh="SM_Box_C10"
-      />
-    </body>
-    <body
-      name="SM_Box_B8"
-      pos="-16.699996948242188 57.20866012573242 1.6392604112625122"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_B8 geom"
-        mesh="SM_Box_B8"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Railing_Corner6_20_GEN_VARIABLE_StaticMeshActor_CAT_486"
-      pos="18.93362045288086 19.601991653442383 2.220768690109253"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Railing_Corner6_20_GEN_VARIABLE_StaticMeshActor_CAT_486 geom"
-        mesh="SM_Railing_Corner6_20_GEN_VARIABLE_StaticMeshActor_CAT_486"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_DockingStation_Stairs2_GEN_VARIABLE_StaticMeshActor_CAT_7726"
-      pos="-7.349991321563721 27.711814880371094 2.0999999046325684"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DockingStation_Stairs2_GEN_VARIABLE_StaticMeshActor_CAT_7726 geom"
-        mesh="SM_DockingStation_Stairs2_GEN_VARIABLE_StaticMeshActor_CAT_7726"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_DockingStation_Top_50_GEN_VARIABLE_StaticMeshActor_CAT_7711"
-      pos="0.7765927314758301 31.804637908935547 5.241786003112793"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DockingStation_Top_50_GEN_VARIABLE_StaticMeshActor_CAT_7711 geom"
-        mesh="SM_DockingStation_Top_50_GEN_VARIABLE_StaticMeshActor_CAT_7711"
-        material="yellow metal"
-      />
-    </body>
-    <body
-      name="SM_DockingStation_Stairs_46_GEN_VARIABLE_StaticMeshActo_37a21c2"
-      pos="7.349991321563721 27.711814880371094 2.0999999046325684"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DockingStation_Stairs_46_GEN_VARIABLE_StaticMeshActo_37a21c2 geom"
-        mesh="SM_DockingStation_Stairs_46_GEN_VARIABLE_StaticMeshActo_37a21c2"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_DockingStation_438_GEN_VARIABLE_StaticMeshActor_CAT_7694"
-      pos="-1.9280387277831323e-06 28.468456268310547 1.0099992752075195"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DockingStation_438_GEN_VARIABLE_StaticMeshActor_CAT_7694 geom"
-        mesh="SM_DockingStation_438_GEN_VARIABLE_StaticMeshActor_CAT_7694"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_Cart2"
-      pos="7.451595306396484 54.622371673583984 0.8229033350944519"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Cart2 geom"
-        mesh="SM_Cart2"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_TrashBin4"
-      pos="-24.406410217285156 27.068607330322266 0.6565067172050476"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_TrashBin4 geom"
-        mesh="SM_TrashBin4"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallFront_382.003"
-      pos="-2.543131586207892e-06 -1.9413375854492188 12.243973731994629"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallFront_382.003 geom"
-        mesh="SM_WallFront_382.003"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_DoorSlide_399.001"
-      pos="-0.026868795976042747 57.99137496948242 6.278016567230225"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_DoorSlide_399.001 geom"
-        mesh="SM_DoorSlide_399.001"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallSide16.001"
-      pos="25.105857849121094 54.55787658691406 10.189190864562988"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSide16.001 geom"
-        mesh="SM_WallSide16.001"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_WallSide16.002"
-      pos="25.105857849121094 54.55787658691406 10.189191818237305"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSide16.002 geom"
-        mesh="SM_WallSide16.002"
-        material="glass"
-      />
-    </body>
-    <body
-      name="collision_SM_WallSide4.001"
-      pos="-25.105867385864258 32.0594482421875 10.189190864562988"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_WallSide4.001 geom"
-        mesh="collision_SM_WallSide4.001"
-      />
-    </body>
-    <body
-      name="SM_WallSide4.002"
-      pos="-25.105867385864258 32.0594482421875 10.189191818237305"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSide4.002 geom"
-        mesh="SM_WallSide4.002"
-        material="glass"
-      />
-    </body>
-    <body
-      name="SM_Lamps6.001"
-      pos="13.000012397766113 13.308566093444824 14.159097671508789"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Lamps6.001 geom"
-        mesh="SM_Lamps6.001"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallBack_396.001"
-      pos="-0.0011624015169218183 58.21354293823242 11.401654243469238"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallBack_396.001 geom"
-        mesh="SM_WallBack_396.001"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_Roof16_360.001"
-      pos="12.03618335723877 2.0586605072021484 18.08340835571289"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Roof16_360.001 geom"
-        mesh="SM_Roof16_360.001"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_Roof4.001"
-      pos="-12.036163330078125 24.558650970458984 18.083402633666992"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Roof4.001 geom"
-        mesh="SM_Roof4.001"
-        material="walls"
-      />
-    </body>
-    <body
-      name="Cube.003"
-      pos="-5.911145210266113 7.195913791656494 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="Cube.003 geom"
-        mesh="Cube.003"
-        material="yellow_tape"
-      />
-    </body>
-    <body
-      name="collision_Plane"
-      pos="0.0 25.80866050720215 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Plane geom"
-        mesh="collision_Plane"
-      />
-    </body>
-    <body
-      name="collision_Plane.001"
-      pos="0.0 25.80866050720215 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Plane.001 geom"
-        mesh="collision_Plane.001"
-      />
-    </body>
-    <body
-      name="collision_Plane.002"
-      pos="-2.11828875541687 28.654041290283203 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Plane.002 geom"
-        mesh="collision_Plane.002"
-      />
-    </body>
-    <body
-      name="collision_Plane.003"
-      pos="0.02430248260498047 25.80866050720215 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Plane.003 geom"
-        mesh="collision_Plane.003"
-      />
-    </body>
-    <body
-      name="collision_Plane.004"
-      pos="2.1425912380218506 28.654041290283203 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Plane.004 geom"
-        mesh="collision_Plane.004"
-      />
-    </body>
-    <body
-      name="collision_Plane.005"
-      pos="0.021426012739539146 33.11489486694336 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Plane.005 geom"
-        mesh="collision_Plane.005"
-      />
-    </body>
-    <body
-      name="collision_Plane.006"
-      pos="0.021426014602184296 28.923986434936523 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Plane.006 geom"
-        mesh="collision_Plane.006"
-      />
-    </body>
-    <body
-      name="collision_Plane.007"
-      pos="7.323379039764404 29.0782527923584 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Plane.007 geom"
-        mesh="collision_Plane.007"
-      />
-    </body>
-    <body
-      name="collision_Plane.008"
-      pos="-7.353374481201172 29.0782527923584 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Plane.008 geom"
-        mesh="collision_Plane.008"
-      />
-    </body>
-    <body
-      name="collision_Plane.009"
-      pos="-3.717996597290039 26.18421173095703 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Plane.009 geom"
-        mesh="collision_Plane.009"
-      />
-    </body>
-    <body
-      name="collision_Cube"
-      pos="-11.642203330993652 1.8356692790985107 0.43372729420661926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube geom"
-        mesh="collision_Cube"
-      />
-      <body
-        name="SM_Cart_28"
-        pos="0.0004825592041015625 0.033867597579956055 -0.009417742490768433"
-        quat="1.0 0.0 0.0 0.0"
-      >
-        <geom
-          class="visual"
-          type="mesh"
-          name="SM_Cart_28 geom"
-          mesh="SM_Cart_28"
-          material="metal_hangar"
-        />
+    <body name="SM_Shelf_A2" pos="23.998563766479492 8.312459945678711 1.1799222230911255" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Shelf_A2 geom" mesh="SM_Shelf_A2" material="metal_hangar"/>
+    </body>
+    <body name="SM_Shelf_A3" pos="-24.00143814086914 15.81245231628418 1.1799222230911255" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Shelf_A3 geom" mesh="SM_Shelf_A3" material="metal_hangar"/>
+    </body>
+    <body name="SM_Shelf_B_39" pos="-23.850627899169922 10.819804191589355 0.9544692635536194" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Shelf_B_39 geom" mesh="SM_Shelf_B_39" material="metal_hangar"/>
+    </body>
+    <body name="SM_Shelf_B2" pos="24.149372100830078 11.319804191589355 0.9544692635536194" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Shelf_B2 geom" mesh="SM_Shelf_B2" material="metal_hangar"/>
+    </body>
+    <body name="SM_Shelf_B3" pos="24.149372100830078 15.819796562194824 0.9544692635536194" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Shelf_B3 geom" mesh="SM_Shelf_B3" material="metal_hangar"/>
+    </body>
+    <body name="SM_Box_A_44" pos="-20.89999771118164 54.308658599853516 5.436284065246582" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_A_44 geom" mesh="SM_Box_A_44" material="box"/>
+    </body>
+    <body name="SM_Box_A2" pos="-18.699996948242188 54.308658599853516 5.436284065246582" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_A2 geom" mesh="SM_Box_A2" material="box"/>
+    </body>
+    <body name="SM_Box_A3" pos="-16.499998092651367 54.308658599853516 5.436284065246582" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_A3 geom" mesh="SM_Box_A3" material="box"/>
+    </body>
+    <body name="SM_Box_A4" pos="-20.89999771118164 51.60866165161133 5.436284065246582" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_A4 geom" mesh="SM_Box_A4" material="box"/>
+    </body>
+    <body name="SM_Box_A5" pos="-18.699996948242188 51.60866165161133 5.436284065246582" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_A5 geom" mesh="SM_Box_A5" material="box"/>
+    </body>
+    <body name="SM_Box_A6" pos="-16.499998092651367 51.60866165161133 5.436284065246582" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_A6 geom" mesh="SM_Box_A6" material="box"/>
+    </body>
+    <body name="SM_Box_B_54" pos="-14.19999885559082 53.70866012573242 5.239261150360107" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_B_54 geom" mesh="SM_Box_B_54" material="box"/>
+    </body>
+    <body name="SM_Box_B2" pos="-13.299997329711914 53.70866012573242 5.239261150360107" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_B2 geom" mesh="SM_Box_B2" material="box"/>
+    </body>
+    <body name="SM_Box_B3" pos="-12.399999618530273 53.70866012573242 5.239261150360107" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_B3 geom" mesh="SM_Box_B3" material="box"/>
+    </body>
+    <body name="SM_Box_B4" pos="-11.5 53.70866012573242 5.239261150360107" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_B4 geom" mesh="SM_Box_B4" material="box"/>
+    </body>
+    <body name="SM_Box_B5" pos="-10.59999942779541 53.70866012573242 5.239261150360107" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_B5 geom" mesh="SM_Box_B5" material="box"/>
+    </body>
+    <body name="SM_Box_B6" pos="-9.69999885559082 53.70866012573242 5.239261150360107" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_B6 geom" mesh="SM_Box_B6" material="box"/>
+    </body>
+    <body name="SM_Box_B7" pos="-8.800000190734863 53.70866012573242 5.239261150360107" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_B7 geom" mesh="SM_Box_B7" material="box"/>
+    </body>
+    <body name="SM_Box_C_63" pos="-23.299999237060547 46.808658599853516 4.965346336364746" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_C_63 geom" mesh="SM_Box_C_63" material="box"/>
+    </body>
+    <body name="SM_Box_C2" pos="-21.999998092651367 46.808658599853516 4.965346336364746" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_C2 geom" mesh="SM_Box_C2" material="box"/>
+    </body>
+    <body name="SM_Box_C3" pos="-20.699996948242188 46.808658599853516 4.965346336364746" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_C3 geom" mesh="SM_Box_C3" material="box"/>
+    </body>
+    <body name="SM_Box_C4_68" pos="23.500001907348633 53.15865707397461 4.965346336364746" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_C4_68 geom" mesh="SM_Box_C4_68" material="box"/>
+    </body>
+    <body name="SM_Box_C5" pos="22.19999885559082 53.15865707397461 4.965346336364746" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_C5 geom" mesh="SM_Box_C5" material="box"/>
+    </body>
+    <body name="SM_Box_C6" pos="20.900001525878906 53.15865707397461 4.965346336364746" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_C6 geom" mesh="SM_Box_C6" material="box"/>
+    </body>
+    <body name="collision_SM_Box_A7_73" pos="-22.699996948242188 0.2086658477783203 1.936284065246582" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Box_A7_73 geom" mesh="collision_SM_Box_A7_73"/>
+    </body>
+    <body name="collision_SM_Box_A8" pos="-22.699996948242188 2.908662796020508 1.936284065246582" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Box_A8 geom" mesh="collision_SM_Box_A8"/>
+    </body>
+    <body name="collision_SM_Box_C7_77" pos="-20.899995803833008 1.1086597442626953 1.3653459548950195" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Box_C7_77 geom" mesh="collision_SM_Box_C7_77"/>
+    </body>
+    <body name="SM_Room2_3" pos="-17.25 48.308658599853516 2.600001335144043" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Room2_3 geom" mesh="SM_Room2_3" material="walls"/>
+    </body>
+    <body name="SM_Pillar_10" pos="-9.499999046325684 42.808658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Pillar_10 geom" mesh="SM_Pillar_10" material="walls"/>
+    </body>
+    <body name="SM_Pillar2" pos="-9.499999046325684 46.308658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Pillar2 geom" mesh="SM_Pillar2" material="walls"/>
+    </body>
+    <body name="SM_Pillar3" pos="-9.499999046325684 50.308658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Pillar3 geom" mesh="SM_Pillar3" material="walls"/>
+    </body>
+    <body name="SM_Pillar4" pos="-9.499999046325684 53.808658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Pillar4 geom" mesh="SM_Pillar4" material="walls"/>
+    </body>
+    <body name="SM_Pillar5" pos="9.000000953674316 42.808658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Pillar5 geom" mesh="SM_Pillar5" material="walls"/>
+    </body>
+    <body name="SM_Pillar6" pos="9.000000953674316 46.308658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Pillar6 geom" mesh="SM_Pillar6" material="walls"/>
+    </body>
+    <body name="SM_Pillar7" pos="9.000000953674316 50.308658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Pillar7 geom" mesh="SM_Pillar7" material="walls"/>
+    </body>
+    <body name="SM_Pillar8" pos="9.000000953674316 53.808658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Pillar8 geom" mesh="SM_Pillar8" material="walls"/>
+    </body>
+    <body name="SM_DoorSingle3" pos="11.854381561279297 48.067501068115234 1.0016013383865356" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DoorSingle3 geom" mesh="SM_DoorSingle3" material="metal_hangar"/>
+    </body>
+    <body name="SM_DoorDouble_26" pos="25.2814998626709 24.80866050720215 1.0649999380111694" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DoorDouble_26 geom" mesh="SM_DoorDouble_26" material="metal_hangar"/>
+    </body>
+    <body name="SM_DoorDouble2" pos="-25.000579833984375 24.30866050720215 1.0649999380111694" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DoorDouble2 geom" mesh="SM_DoorDouble2" material="metal_hangar"/>
+    </body>
+    <body name="SM_DoorSingle4" pos="21.258838653564453 -1.6457252502441406 1.0016013383865356" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DoorSingle4 geom" mesh="SM_DoorSingle4" material="metal_hangar"/>
+    </body>
+    <body name="SM_Stairs_32" pos="17.748929977416992 39.79243087768555 1.639772653579712" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Stairs_32 geom" mesh="SM_Stairs_32" material="walls"/>
+    </body>
+    <body name="SM_Stairs2" pos="-17.748912811279297 39.792415618896484 1.6397724151611328" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Stairs2 geom" mesh="SM_Stairs2" material="walls"/>
+    </body>
+    <body name="SM_StairsRailing_Inner_2" pos="18.00760841369629 40.89141845703125 2.7805166244506836" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_StairsRailing_Inner_2 geom" mesh="SM_StairsRailing_Inner_2" material="metal_hangar"/>
+    </body>
+    <body name="SM_StairsRailing_Outer_7" pos="18.999317169189453 38.7586784362793 2.959934949874878" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_StairsRailing_Outer_7 geom" mesh="SM_StairsRailing_Outer_7" material="metal_hangar"/>
+    </body>
+    <body name="SM_StairsRailing_Top_10" pos="14.234500885009766 41.46199035644531 4.4237823486328125" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_StairsRailing_Top_10 geom" mesh="SM_StairsRailing_Top_10" material="metal_hangar"/>
+    </body>
+    <body name="SM_StairsRailing_Outer2" pos="-18.999317169189453 38.75865173339844 2.959934949874878" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_StairsRailing_Outer2 geom" mesh="SM_StairsRailing_Outer2" material="metal_hangar"/>
+    </body>
+    <body name="SM_StairsRailing_Inner2" pos="-18.057634353637695 40.89144515991211 2.7805166244506836" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_StairsRailing_Inner2 geom" mesh="SM_StairsRailing_Inner2" material="metal_hangar"/>
+    </body>
+    <body name="SM_StairsRailing_Top2_15" pos="-14.334492683410645 41.45530319213867 4.4237823486328125" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_StairsRailing_Top2_15 geom" mesh="SM_StairsRailing_Top2_15" material="metal_hangar"/>
+    </body>
+    <body name="SM_DoorSingle5" pos="-21.458837509155273 41.26304626464844 1.0016013383865356" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DoorSingle5 geom" mesh="SM_DoorSingle5" material="metal_hangar"/>
+    </body>
+    <body name="SM_DoorSingle6" pos="20.941164016723633 41.26304626464844 1.0016013383865356" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DoorSingle6 geom" mesh="SM_DoorSingle6" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H_2" pos="0.0 28.20865821838379 19.900009155273438" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H_2 geom" mesh="SM_WallRailing_H_2" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H2_5" pos="2.500000238418579 28.20865821838379 19.400009155273438" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H2_5 geom" mesh="SM_WallRailing_H2_5" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H3" pos="5.0 28.20865821838379 19.000009536743164" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H3 geom" mesh="SM_WallRailing_H3" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H4" pos="7.499999523162842 28.20865821838379 18.500009536743164" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H4 geom" mesh="SM_WallRailing_H4" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H5" pos="9.999999046325684 28.20865821838379 18.10000991821289" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H5 geom" mesh="SM_WallRailing_H5" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H6" pos="12.499999046325684 28.20865821838379 17.60000991821289" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H6 geom" mesh="SM_WallRailing_H6" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H7" pos="14.999999046325684 28.20865821838379 17.200008392333984" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H7 geom" mesh="SM_WallRailing_H7" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H8" pos="17.5 28.20865821838379 16.80000877380371" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H8 geom" mesh="SM_WallRailing_H8" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H9" pos="20.0 28.20865821838379 16.30000877380371" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H9 geom" mesh="SM_WallRailing_H9" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H10" pos="22.500001907348633 28.20865821838379 15.900009155273438" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H10 geom" mesh="SM_WallRailing_H10" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H11" pos="-2.499999761581421 28.20865821838379 19.400009155273438" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H11 geom" mesh="SM_WallRailing_H11" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H12" pos="-5.0 28.20865821838379 19.000009536743164" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H12 geom" mesh="SM_WallRailing_H12" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H13" pos="-7.500000476837158 28.20865821838379 18.500009536743164" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H13 geom" mesh="SM_WallRailing_H13" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H14" pos="-10.000000953674316 28.20865821838379 18.10000991821289" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H14 geom" mesh="SM_WallRailing_H14" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H15" pos="-12.500000953674316 28.20865821838379 17.60000991821289" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H15 geom" mesh="SM_WallRailing_H15" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H16" pos="-15.000000953674316 28.20865821838379 17.200008392333984" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H16 geom" mesh="SM_WallRailing_H16" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H17" pos="-17.5 28.20865821838379 16.80000877380371" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H17 geom" mesh="SM_WallRailing_H17" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H18" pos="-20.0 28.20865821838379 16.400009155273438" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H18 geom" mesh="SM_WallRailing_H18" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallRailing_H19" pos="-22.499998092651367 28.20865821838379 15.900009155273438" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallRailing_H19 geom" mesh="SM_WallRailing_H19" material="metal_hangar"/>
+    </body>
+    <body name="SM_DoorDouble3" pos="24.936420440673828 24.30866050720215 1.0649999380111694" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DoorDouble3 geom" mesh="SM_DoorDouble3" material="metal_hangar"/>
+    </body>
+    <body name="SM_AccessStand2" pos="11.576964378356934 9.308902740478516 0.8804012537002563" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_AccessStand2 geom" mesh="SM_AccessStand2" material="yellow metal"/>
+    </body>
+    <body name="SM_Ladder2" pos="-23.856979370117188 32.41215515136719 0.9320219159126282" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Ladder2 geom" mesh="SM_Ladder2" material="metal_hangar"/>
+    </body>
+    <body name="SM_Ladder3" pos="-23.48493003845215 31.00689125061035 0.9328975081443787" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Ladder3 geom" mesh="SM_Ladder3" material="metal_hangar"/>
+    </body>
+    <body name="SM_TrashBin2" pos="24.357282638549805 30.5086612701416 0.6565067172050476" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_TrashBin2 geom" mesh="SM_TrashBin2" material="metal_hangar"/>
+    </body>
+    <body name="SM_Box_C8" pos="20.941354751586914 56.158653259277344 1.3647199869155884" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_C8 geom" mesh="SM_Box_C8"/>
+    </body>
+    <body name="SM_Box_C9" pos="20.641357421875 57.458656311035156 1.26472008228302" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_C9 geom" mesh="SM_Box_C9"/>
+    </body>
+    <body name="SM_Box_C10" pos="-20.699996948242188 56.70866012573242 1.3653464317321777" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_C10 geom" mesh="SM_Box_C10"/>
+    </body>
+    <body name="SM_Box_B8" pos="-16.699996948242188 57.20866012573242 1.6392604112625122" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_B8 geom" mesh="SM_Box_B8" material="box"/>
+    </body>
+    <body name="SM_Railing_Corner6_20_GEN_VARIABLE_StaticMeshActor_CAT_486" pos="18.93362045288086 19.601991653442383 2.220768690109253" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Railing_Corner6_20_GEN_VARIABLE_StaticMeshActor_CAT_486 geom" mesh="SM_Railing_Corner6_20_GEN_VARIABLE_StaticMeshActor_CAT_486" material="metal_hangar"/>
+    </body>
+    <body name="SM_DockingStation_Stairs2_GEN_VARIABLE_StaticMeshActor_CAT_7726" pos="-7.349991321563721 27.711814880371094 2.0999999046325684" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DockingStation_Stairs2_GEN_VARIABLE_StaticMeshActor_CAT_7726 geom" mesh="SM_DockingStation_Stairs2_GEN_VARIABLE_StaticMeshActor_CAT_7726" material="metal_hangar"/>
+    </body>
+    <body name="SM_DockingStation_Top_50_GEN_VARIABLE_StaticMeshActor_CAT_7711" pos="0.7765927314758301 31.804637908935547 5.241786003112793" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DockingStation_Top_50_GEN_VARIABLE_StaticMeshActor_CAT_7711 geom" mesh="SM_DockingStation_Top_50_GEN_VARIABLE_StaticMeshActor_CAT_7711" material="yellow metal"/>
+    </body>
+    <body name="SM_DockingStation_Stairs_46_GEN_VARIABLE_StaticMeshActo_37a21c2" pos="7.349991321563721 27.711814880371094 2.0999999046325684" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DockingStation_Stairs_46_GEN_VARIABLE_StaticMeshActo_37a21c2 geom" mesh="SM_DockingStation_Stairs_46_GEN_VARIABLE_StaticMeshActo_37a21c2" material="metal_hangar"/>
+    </body>
+    <body name="SM_DockingStation_438_GEN_VARIABLE_StaticMeshActor_CAT_7694" pos="-1.9280387277831323e-06 28.468456268310547 1.0099992752075195" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DockingStation_438_GEN_VARIABLE_StaticMeshActor_CAT_7694 geom" mesh="SM_DockingStation_438_GEN_VARIABLE_StaticMeshActor_CAT_7694" material="metal_hangar"/>
+    </body>
+    <body name="SM_Cart2" pos="7.451595306396484 54.622371673583984 0.8229033350944519" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Cart2 geom" mesh="SM_Cart2" material="metal_hangar"/>
+    </body>
+    <body name="SM_TrashBin4" pos="-24.406410217285156 27.068607330322266 0.6565067172050476" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_TrashBin4 geom" mesh="SM_TrashBin4" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallFront_382.003" pos="-2.543131586207892e-06 -1.9413375854492188 12.243973731994629" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallFront_382.003 geom" mesh="SM_WallFront_382.003" material="walls"/>
+    </body>
+    <body name="SM_DoorSlide_399.001" pos="-0.026868795976042747 57.99137496948242 6.278016567230225" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_DoorSlide_399.001 geom" mesh="SM_DoorSlide_399.001" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallSide16.001" pos="25.105857849121094 54.55787658691406 10.189190864562988" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSide16.001 geom" mesh="SM_WallSide16.001" material="walls"/>
+    </body>
+    <body name="SM_WallSide16.002" pos="25.105857849121094 54.55787658691406 10.189191818237305" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSide16.002 geom" mesh="SM_WallSide16.002" material="glass"/>
+    </body>
+    <body name="collision_SM_WallSide4.001" pos="-25.105867385864258 32.0594482421875 10.189190864562988" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_WallSide4.001 geom" mesh="collision_SM_WallSide4.001"/>
+    </body>
+    <body name="SM_WallSide4.002" pos="-25.105867385864258 32.0594482421875 10.189191818237305" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSide4.002 geom" mesh="SM_WallSide4.002" material="glass"/>
+    </body>
+    <body name="SM_Lamps6.001" pos="13.000012397766113 13.308566093444824 14.159097671508789" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Lamps6.001 geom" mesh="SM_Lamps6.001" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallBack_396.001" pos="-0.0011624015169218183 58.21354293823242 11.401654243469238" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallBack_396.001 geom" mesh="SM_WallBack_396.001" material="walls"/>
+    </body>
+    <body name="SM_Roof16_360.001" pos="12.03618335723877 2.0586605072021484 18.08340835571289" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Roof16_360.001 geom" mesh="SM_Roof16_360.001" material="walls"/>
+    </body>
+    <body name="SM_Roof4.001" pos="-12.036163330078125 24.558650970458984 18.083402633666992" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Roof4.001 geom" mesh="SM_Roof4.001" material="walls"/>
+    </body>
+    <body name="Cube.003" pos="-5.911145210266113 7.195913791656494 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="Cube.003 geom" mesh="Cube.003" material="yellow_tape"/>
+    </body>
+    <body name="collision_Plane" pos="0.0 25.80866050720215 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Plane geom" mesh="collision_Plane"/>
+    </body>
+    <body name="collision_Plane.001" pos="0.0 25.80866050720215 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Plane.001 geom" mesh="collision_Plane.001"/>
+    </body>
+    <body name="collision_Plane.002" pos="-2.11828875541687 28.654041290283203 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Plane.002 geom" mesh="collision_Plane.002"/>
+    </body>
+    <body name="collision_Plane.003" pos="0.02430248260498047 25.80866050720215 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Plane.003 geom" mesh="collision_Plane.003"/>
+    </body>
+    <body name="collision_Plane.004" pos="2.1425912380218506 28.654041290283203 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Plane.004 geom" mesh="collision_Plane.004"/>
+    </body>
+    <body name="collision_Plane.005" pos="0.021426012739539146 33.11489486694336 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Plane.005 geom" mesh="collision_Plane.005"/>
+    </body>
+    <body name="collision_Plane.006" pos="0.021426014602184296 28.923986434936523 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Plane.006 geom" mesh="collision_Plane.006"/>
+    </body>
+    <body name="collision_Plane.007" pos="7.323379039764404 29.0782527923584 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Plane.007 geom" mesh="collision_Plane.007"/>
+    </body>
+    <body name="collision_Plane.008" pos="-7.353374481201172 29.0782527923584 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Plane.008 geom" mesh="collision_Plane.008"/>
+    </body>
+    <body name="collision_Plane.009" pos="-3.717996597290039 26.18421173095703 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Plane.009 geom" mesh="collision_Plane.009"/>
+    </body>
+    <body name="collision_Cube" pos="-11.642203330993652 1.8356692790985107 0.43372729420661926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube geom" mesh="collision_Cube"/>
+      <body name="SM_Cart_28" pos="0.0004825592041015625 0.033867597579956055 -0.009417742490768433" quat="1.0 0.0 0.0 0.0">
+        <geom class="visual" type="mesh" name="SM_Cart_28 geom" mesh="SM_Cart_28" material="metal_hangar"/>
       </body>
     </body>
-    <body
-      name="collision_Cube.001"
-      pos="11.559602737426758 9.308443069458008 0.800389289855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.001 geom"
-        mesh="collision_Cube.001"
-      />
-    </body>
-    <body
-      name="collision_Cube.002"
-      pos="-19.29080581665039 16.40394401550293 0.800389289855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.002 geom"
-        mesh="collision_Cube.002"
-      />
-    </body>
-    <body
-      name="collision_Cube.004"
-      pos="-22.69596290588379 8.191585540771484 0.800389289855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.004 geom"
-        mesh="collision_Cube.004"
-      />
-    </body>
-    <body
-      name="collision_Cube.005"
-      pos="-23.86355972290039 10.825281143188477 0.9106760025024414"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.005 geom"
-        mesh="collision_Cube.005"
-      />
-    </body>
-    <body
-      name="collision_Cube.006"
-      pos="-24.01235008239746 15.830931663513184 1.0179872512817383"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.006 geom"
-        mesh="collision_Cube.006"
-      />
-    </body>
-    <body
-      name="collision_Cube.007"
-      pos="24.00054931640625 8.311418533325195 1.0179872512817383"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.007 geom"
-        mesh="collision_Cube.007"
-      />
-    </body>
-    <body
-      name="collision_Cube.008"
-      pos="24.14933967590332 11.336413383483887 1.0179872512817383"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.008 geom"
-        mesh="collision_Cube.008"
-      />
-    </body>
-    <body
-      name="collision_Cube.009"
-      pos="24.14933967590332 15.816640853881836 1.0179872512817383"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.009 geom"
-        mesh="collision_Cube.009"
-      />
-    </body>
-    <body
-      name="collision_Cube.010"
-      pos="-23.473466873168945 31.006977081298828 0.800389289855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.010 geom"
-        mesh="collision_Cube.010"
-      />
-    </body>
-    <body
-      name="collision_Cube.011"
-      pos="-23.901172637939453 32.4188232421875 0.800389289855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.011 geom"
-        mesh="collision_Cube.011"
-      />
-    </body>
-    <body
-      name="collision_Cube.012"
-      pos="-24.305641174316406 27.054115295410156 0.4974985122680664"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.012 geom"
-        mesh="collision_Cube.012"
-      />
-    </body>
-    <body
-      name="collision_Cube.013"
-      pos="24.2586669921875 29.708847045898438 0.4974985122680664"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.013 geom"
-        mesh="collision_Cube.013"
-      />
-    </body>
-    <body
-      name="collision_Cube.014"
-      pos="24.2586669921875 30.51011085510254 0.4974985122680664"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.014 geom"
-        mesh="collision_Cube.014"
-      />
-    </body>
-    <body
-      name="collision_Cube.015"
-      pos="7.401407241821289 54.594669342041016 0.7229089736938477"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube.015 geom"
-        mesh="collision_Cube.015"
-      />
-    </body>
-    <body
-      name="collision_SM_Room_426.001"
-      pos="17.14999771118164 48.308658599853516 2.59999942779541"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Room_426.001 geom"
-        mesh="collision_SM_Room_426.001"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_Room_426.002"
-      pos="-19.69150733947754 48.308658599853516 2.59999942779541"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Room_426.002 geom"
-        mesh="SM_Room_426.002"
-        material="walls"
-      />
-    </body>
-    <body
-      name="collision_Plane.010"
-      pos="0.0 25.80866050720215 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Plane.010 geom"
-        mesh="collision_Plane.010"
-      />
-    </body>
-    <body
-      name="collision_Plane.011"
-      pos="0.0 25.80866050720215 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Plane.011 geom"
-        mesh="collision_Plane.011"
-      />
-    </body>
-    <body
-      name="collision_Plane.012"
-      pos="0.0 25.80866050720215 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Plane.012 geom"
-        mesh="collision_Plane.012"
-      />
-    </body>
-    <body
-      name="collision_supports"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports geom"
-        mesh="collision_supports"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.001"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.001 geom"
-        mesh="collision_supports.001"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.002"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.002 geom"
-        mesh="collision_supports.002"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.003"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.003 geom"
-        mesh="collision_supports.003"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.004"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.004 geom"
-        mesh="collision_supports.004"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.005"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.005 geom"
-        mesh="collision_supports.005"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.006"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.006 geom"
-        mesh="collision_supports.006"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.007"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.007 geom"
-        mesh="collision_supports.007"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.008"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.008 geom"
-        mesh="collision_supports.008"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.009"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.009 geom"
-        mesh="collision_supports.009"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.010"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.010 geom"
-        mesh="collision_supports.010"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.011"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.011 geom"
-        mesh="collision_supports.011"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.012"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.012 geom"
-        mesh="collision_supports.012"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.013"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.013 geom"
-        mesh="collision_supports.013"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.014"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.014 geom"
-        mesh="collision_supports.014"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="collision_supports.015"
-      pos="-18.47951889038086 20.80866813659668 12.532681465148926"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_supports.015 geom"
-        mesh="collision_supports.015"
-        material="metal_hangar"
-      />
-    </body>
-    <body
-      name="SM_WallBack_396.002"
-      pos="-0.0011624015169218183 58.21354293823242 11.401654243469238"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallBack_396.002 geom"
-        mesh="SM_WallBack_396.002"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_WallBack_396.003"
-      pos="-0.0011624015169218183 58.21354293823242 11.401654243469238"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallBack_396.003 geom"
-        mesh="SM_WallBack_396.003"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_WallFront_382.002"
-      pos="-2.543131586207892e-06 -1.9413375854492188 12.243975639343262"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallFront_382.002 geom"
-        mesh="SM_WallFront_382.002"
-        material="walls"
-      />
-    </body>
-    <body
-      name="SM_WallFront_382.00311"
-      pos="-2.543131586207892e-06 -1.9413375854492188 12.243973731994629"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallFront_382.00311 geom"
-        mesh="SM_WallFront_382.00311"
-        material="walls"
-      />
-    </body>
-    <body
-      name="collision_SM_Room_426.003"
-      pos="-18.11935043334961 40.011077880859375 2.59999942779541"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Room_426.003 geom"
-        mesh="collision_SM_Room_426.003"
-        material="walls"
-      />
-    </body>
-    <body
-      name="collision_SM_Room_426.004"
-      pos="16.634126663208008 40.011077880859375 2.59999942779541"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Room_426.004 geom"
-        mesh="collision_SM_Room_426.004"
-        material="walls"
-      />
-    </body>
-    <body
-      name="collision_SM_Floor_376"
-      pos="0.0 25.80866050720215 -0.5"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Floor_376 geom"
-        mesh="collision_SM_Floor_376"
-      />
-    </body>
-    <body
-      name="SM_WallSide4.001"
-      pos="-25.105867385864258 32.0594482421875 10.189190864562988"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_WallSide4.001 geom"
-        mesh="SM_WallSide4.001"
-        material="walls"
-      />
-    </body>
-    <body
-      name="collision_SM_WallBack_396.001"
-      pos="-0.0011624015169218183 58.21354293823242 11.401654243469238"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_WallBack_396.001 geom"
-        mesh="collision_SM_WallBack_396.001"
-      />
-    </body>
-    <body
-      name="collision_SM_WallBack_396.002"
-      pos="-0.0011624015169218183 58.21354293823242 11.401654243469238"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_WallBack_396.002 geom"
-        mesh="collision_SM_WallBack_396.002"
-      />
-    </body>
-    <body
-      name="collision_SM_WallSide16.002"
-      pos="25.105857849121094 54.55787658691406 10.189190864562988"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_WallSide16.002 geom"
-        mesh="collision_SM_WallSide16.002"
-      />
-    </body>
-    <body
-      name="collision_SM_WallFront_382.001"
-      pos="-2.543131586207892e-06 -1.9413375854492188 12.243973731994629"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_WallFront_382.001 geom"
-        mesh="collision_SM_WallFront_382.001"
-      />
-    </body>
-    <body
-      name="collision_SM_WallFront_382.002"
-      pos="-2.543131586207892e-06 -1.9413375854492188 12.243973731994629"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_WallFront_382.002 geom"
-        mesh="collision_SM_WallFront_382.002"
-      />
-    </body>
-    <body
-      name="SM_Box_A7_73.001"
-      pos="-22.699996948242188 0.2086658477783203 1.936284065246582"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_A7_73.001 geom"
-        mesh="SM_Box_A7_73.001"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_A8.001"
-      pos="-22.699996948242188 2.908662796020508 1.936284065246582"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_A8.001 geom"
-        mesh="SM_Box_A8.001"
-        material="box"
-      />
-    </body>
-    <body
-      name="SM_Box_C7_77.001"
-      pos="-20.899995803833008 1.1086597442626953 1.3653459548950195"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="SM_Box_C7_77.001 geom"
-        mesh="SM_Box_C7_77.001"
-        material="box"
-      />
-    </body>
-    <body
-      name="collision_SM_Pillar4.001"
-      pos="-9.499999046325684 53.808658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Pillar4.001 geom"
-        mesh="collision_SM_Pillar4.001"
-      />
-    </body>
-    <body
-      name="collision_SM_Pillar3.001"
-      pos="-9.499999046325684 50.308658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Pillar3.001 geom"
-        mesh="collision_SM_Pillar3.001"
-      />
-    </body>
-    <body
-      name="collision_SM_Pillar2.001"
-      pos="-9.499999046325684 46.308658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Pillar2.001 geom"
-        mesh="collision_SM_Pillar2.001"
-      />
-    </body>
-    <body
-      name="collision_SM_Pillar_10.001"
-      pos="-9.499999046325684 42.808658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Pillar_10.001 geom"
-        mesh="collision_SM_Pillar_10.001"
-      />
-    </body>
-    <body
-      name="collision_SM_Pillar8.001"
-      pos="9.000000953674316 53.808658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Pillar8.001 geom"
-        mesh="collision_SM_Pillar8.001"
-      />
-    </body>
-    <body
-      name="collision_SM_Pillar7.001"
-      pos="9.000000953674316 50.308658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Pillar7.001 geom"
-        mesh="collision_SM_Pillar7.001"
-      />
-    </body>
-    <body
-      name="collision_SM_Pillar6.001"
-      pos="9.000000953674316 46.308658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Pillar6.001 geom"
-        mesh="collision_SM_Pillar6.001"
-      />
-    </body>
-    <body
-      name="collision_SM_Pillar5.001"
-      pos="9.000000953674316 42.808658599853516 1.649998664855957"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_SM_Pillar5.001 geom"
-        mesh="collision_SM_Pillar5.001"
-      />
-    </body>
-    <body
-      name="collision_Cube_002.001"
-      pos="-11.812490463256836 2.936809539794922 0.800389289855957"
-      quat="4.371138828673793e-08 0.0 0.0 -1.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_Cube_002.001 geom"
-        mesh="collision_Cube_002.001"
-      />
-      <body
-        name="SM_AccessStand_441.001"
-        pos="-0.0157470703125 0.0010638236999511719 0.08000737428665161"
-        quat="1.0 0.0 0.0 0.0"
-      >
-        <geom
-          class="visual"
-          type="mesh"
-          name="SM_AccessStand_441.001 geom"
-          mesh="SM_AccessStand_441.001"
-          material="yellow metal"
-        />
+    <body name="collision_Cube.001" pos="11.559602737426758 9.308443069458008 0.800389289855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.001 geom" mesh="collision_Cube.001"/>
+    </body>
+    <body name="collision_Cube.002" pos="-19.29080581665039 16.40394401550293 0.800389289855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.002 geom" mesh="collision_Cube.002"/>
+    </body>
+    <body name="collision_Cube.004" pos="-22.69596290588379 8.191585540771484 0.800389289855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.004 geom" mesh="collision_Cube.004"/>
+    </body>
+    <body name="collision_Cube.005" pos="-23.86355972290039 10.825281143188477 0.9106760025024414" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.005 geom" mesh="collision_Cube.005"/>
+    </body>
+    <body name="collision_Cube.006" pos="-24.01235008239746 15.830931663513184 1.0179872512817383" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.006 geom" mesh="collision_Cube.006"/>
+    </body>
+    <body name="collision_Cube.007" pos="24.00054931640625 8.311418533325195 1.0179872512817383" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.007 geom" mesh="collision_Cube.007"/>
+    </body>
+    <body name="collision_Cube.008" pos="24.14933967590332 11.336413383483887 1.0179872512817383" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.008 geom" mesh="collision_Cube.008"/>
+    </body>
+    <body name="collision_Cube.009" pos="24.14933967590332 15.816640853881836 1.0179872512817383" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.009 geom" mesh="collision_Cube.009"/>
+    </body>
+    <body name="collision_Cube.010" pos="-23.473466873168945 31.006977081298828 0.800389289855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.010 geom" mesh="collision_Cube.010"/>
+    </body>
+    <body name="collision_Cube.011" pos="-23.901172637939453 32.4188232421875 0.800389289855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.011 geom" mesh="collision_Cube.011"/>
+    </body>
+    <body name="collision_Cube.012" pos="-24.305641174316406 27.054115295410156 0.4974985122680664" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.012 geom" mesh="collision_Cube.012"/>
+    </body>
+    <body name="collision_Cube.013" pos="24.2586669921875 29.708847045898438 0.4974985122680664" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.013 geom" mesh="collision_Cube.013"/>
+    </body>
+    <body name="collision_Cube.014" pos="24.2586669921875 30.51011085510254 0.4974985122680664" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.014 geom" mesh="collision_Cube.014"/>
+    </body>
+    <body name="collision_Cube.015" pos="7.401407241821289 54.594669342041016 0.7229089736938477" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Cube.015 geom" mesh="collision_Cube.015"/>
+    </body>
+    <body name="collision_SM_Room_426.001" pos="17.14999771118164 48.308658599853516 2.59999942779541" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Room_426.001 geom" mesh="collision_SM_Room_426.001" material="walls"/>
+    </body>
+    <body name="SM_Room_426.002" pos="-19.69150733947754 48.308658599853516 2.59999942779541" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Room_426.002 geom" mesh="SM_Room_426.002" material="walls"/>
+    </body>
+    <body name="collision_Plane.010" pos="0.0 25.80866050720215 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Plane.010 geom" mesh="collision_Plane.010"/>
+    </body>
+    <body name="collision_Plane.011" pos="0.0 25.80866050720215 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Plane.011 geom" mesh="collision_Plane.011"/>
+    </body>
+    <body name="collision_Plane.012" pos="0.0 25.80866050720215 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_Plane.012 geom" mesh="collision_Plane.012"/>
+    </body>
+    <body name="collision_supports" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports geom" mesh="collision_supports" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.001" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.001 geom" mesh="collision_supports.001" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.002" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.002 geom" mesh="collision_supports.002" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.003" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.003 geom" mesh="collision_supports.003" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.004" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.004 geom" mesh="collision_supports.004" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.005" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.005 geom" mesh="collision_supports.005" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.006" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.006 geom" mesh="collision_supports.006" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.007" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.007 geom" mesh="collision_supports.007" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.008" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.008 geom" mesh="collision_supports.008" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.009" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.009 geom" mesh="collision_supports.009" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.010" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.010 geom" mesh="collision_supports.010" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.011" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.011 geom" mesh="collision_supports.011" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.012" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.012 geom" mesh="collision_supports.012" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.013" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.013 geom" mesh="collision_supports.013" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.014" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.014 geom" mesh="collision_supports.014" material="metal_hangar"/>
+    </body>
+    <body name="collision_supports.015" pos="-18.47951889038086 20.80866813659668 12.532681465148926" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_supports.015 geom" mesh="collision_supports.015" material="metal_hangar"/>
+    </body>
+    <body name="SM_WallBack_396.002" pos="-0.0011624015169218183 58.21354293823242 11.401654243469238" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallBack_396.002 geom" mesh="SM_WallBack_396.002" material="walls"/>
+    </body>
+    <body name="SM_WallBack_396.003" pos="-0.0011624015169218183 58.21354293823242 11.401654243469238" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallBack_396.003 geom" mesh="SM_WallBack_396.003" material="walls"/>
+    </body>
+    <body name="SM_WallFront_382.002" pos="-2.543131586207892e-06 -1.9413375854492188 12.243975639343262" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallFront_382.002 geom" mesh="SM_WallFront_382.002" material="walls"/>
+    </body>
+    <body name="SM_WallFront_382.00311" pos="-2.543131586207892e-06 -1.9413375854492188 12.243973731994629" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallFront_382.00311 geom" mesh="SM_WallFront_382.00311" material="walls"/>
+    </body>
+    <body name="collision_SM_Room_426.003" pos="-18.11935043334961 40.011077880859375 2.59999942779541" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Room_426.003 geom" mesh="collision_SM_Room_426.003" material="walls"/>
+    </body>
+    <body name="collision_SM_Room_426.004" pos="16.634126663208008 40.011077880859375 2.59999942779541" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Room_426.004 geom" mesh="collision_SM_Room_426.004" material="walls"/>
+    </body>
+    <body name="collision_SM_Floor_376" pos="0.0 25.80866050720215 -0.5" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Floor_376 geom" mesh="collision_SM_Floor_376"/>
+    </body>
+    <body name="SM_WallSide4.001" pos="-25.105867385864258 32.0594482421875 10.189190864562988" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_WallSide4.001 geom" mesh="SM_WallSide4.001" material="walls"/>
+    </body>
+    <body name="collision_SM_WallBack_396.001" pos="-0.0011624015169218183 58.21354293823242 11.401654243469238" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_WallBack_396.001 geom" mesh="collision_SM_WallBack_396.001"/>
+    </body>
+    <body name="collision_SM_WallBack_396.002" pos="-0.0011624015169218183 58.21354293823242 11.401654243469238" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_WallBack_396.002 geom" mesh="collision_SM_WallBack_396.002"/>
+    </body>
+    <body name="collision_SM_WallSide16.002" pos="25.105857849121094 54.55787658691406 10.189190864562988" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_WallSide16.002 geom" mesh="collision_SM_WallSide16.002"/>
+    </body>
+    <body name="collision_SM_WallFront_382.001" pos="-2.543131586207892e-06 -1.9413375854492188 12.243973731994629" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_WallFront_382.001 geom" mesh="collision_SM_WallFront_382.001"/>
+    </body>
+    <body name="collision_SM_WallFront_382.002" pos="-2.543131586207892e-06 -1.9413375854492188 12.243973731994629" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_WallFront_382.002 geom" mesh="collision_SM_WallFront_382.002"/>
+    </body>
+    <body name="SM_Box_A7_73.001" pos="-22.699996948242188 0.2086658477783203 1.936284065246582" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_A7_73.001 geom" mesh="SM_Box_A7_73.001" material="box"/>
+    </body>
+    <body name="SM_Box_A8.001" pos="-22.699996948242188 2.908662796020508 1.936284065246582" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_A8.001 geom" mesh="SM_Box_A8.001" material="box"/>
+    </body>
+    <body name="SM_Box_C7_77.001" pos="-20.899995803833008 1.1086597442626953 1.3653459548950195" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="SM_Box_C7_77.001 geom" mesh="SM_Box_C7_77.001" material="box"/>
+    </body>
+    <body name="collision_SM_Pillar4.001" pos="-9.499999046325684 53.808658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Pillar4.001 geom" mesh="collision_SM_Pillar4.001"/>
+    </body>
+    <body name="collision_SM_Pillar3.001" pos="-9.499999046325684 50.308658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Pillar3.001 geom" mesh="collision_SM_Pillar3.001"/>
+    </body>
+    <body name="collision_SM_Pillar2.001" pos="-9.499999046325684 46.308658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Pillar2.001 geom" mesh="collision_SM_Pillar2.001"/>
+    </body>
+    <body name="collision_SM_Pillar_10.001" pos="-9.499999046325684 42.808658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Pillar_10.001 geom" mesh="collision_SM_Pillar_10.001"/>
+    </body>
+    <body name="collision_SM_Pillar8.001" pos="9.000000953674316 53.808658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Pillar8.001 geom" mesh="collision_SM_Pillar8.001"/>
+    </body>
+    <body name="collision_SM_Pillar7.001" pos="9.000000953674316 50.308658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Pillar7.001 geom" mesh="collision_SM_Pillar7.001"/>
+    </body>
+    <body name="collision_SM_Pillar6.001" pos="9.000000953674316 46.308658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Pillar6.001 geom" mesh="collision_SM_Pillar6.001"/>
+    </body>
+    <body name="collision_SM_Pillar5.001" pos="9.000000953674316 42.808658599853516 1.649998664855957" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_SM_Pillar5.001 geom" mesh="collision_SM_Pillar5.001"/>
+    </body>
+    <body name="collision_Cube_002.001" pos="-11.812490463256836 2.936809539794922 0.800389289855957" quat="4.371138828673793e-08 0.0 0.0 -1.0">
+      <geom class="collision" type="mesh" name="collision_Cube_002.001 geom" mesh="collision_Cube_002.001"/>
+      <body name="SM_AccessStand_441.001" pos="-0.0157470703125 0.0010638236999511719 0.08000737428665161" quat="1.0 0.0 0.0 0.0">
+        <geom class="visual" type="mesh" name="SM_AccessStand_441.001 geom" mesh="SM_AccessStand_441.001" material="yellow metal"/>
       </body>
     </body>
-    <body
-      name="tire_lndg_rear_l_in.109"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.109 geom"
-        mesh="tire_lndg_rear_l_in.109"
-      />
-    </body>
-    <body
-      name="tire_lndg_rear_l_in.001"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.001 geom"
-        mesh="tire_lndg_rear_l_in.001"
-        material="Material.001"
-      />
-    </body>
-    <body
-      name="tire_lndg_rear_l_in.002"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.002 geom"
-        mesh="tire_lndg_rear_l_in.002"
-      />
-    </body>
-    <body
-      name="tire_lndg_rear_l_in.003"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.003 geom"
-        mesh="tire_lndg_rear_l_in.003"
-        material="rubber"
-      />
-    </body>
-    <body
-      name="tire_lndg_rear_l_in.004"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.004 geom"
-        mesh="tire_lndg_rear_l_in.004"
-      />
-    </body>
-    <body
-      name="tire_lndg_rear_l_in.005"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.005 geom"
-        mesh="tire_lndg_rear_l_in.005"
-      />
-    </body>
-    <body
-      name="tire_lndg_rear_l_in.006"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.006 geom"
-        mesh="tire_lndg_rear_l_in.006"
-      />
-    </body>
-    <body
-      name="tire_lndg_rear_l_in.007"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.007 geom"
-        mesh="tire_lndg_rear_l_in.007"
-        material="Material"
-      />
-    </body>
-    <body
-      name="tire_lndg_rear_l_in.008"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.008 geom"
-        mesh="tire_lndg_rear_l_in.008"
-      />
-    </body>
-    <body
-      name="tire_lndg_rear_l_in.009"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.009 geom"
-        mesh="tire_lndg_rear_l_in.009"
-      />
-    </body>
-    <body
-      name="tire_lndg_rear_l_in.010"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.010 geom"
-        mesh="tire_lndg_rear_l_in.010"
-      />
-    </body>
-    <body
-      name="tire_lndg_rear_l_in.011"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.011 geom"
-        mesh="tire_lndg_rear_l_in.011"
-      />
-    </body>
-    <body
-      name="tire_lndg_rear_l_in.012"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.012 geom"
-        mesh="tire_lndg_rear_l_in.012"
-      />
-    </body>
-    <body
-      name="tire_lndg_rear_l_in.013"
-      pos="0.003512459108605981 17.572731018066406 2.220263957977295"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="tire_lndg_rear_l_in.013 geom"
-        mesh="tire_lndg_rear_l_in.013"
-      />
-    </body>
-    <body
-      name="collision_box_3"
-      pos="-9.765350341796875 3.763852119445801 0.1250065267086029"
-      quat="0.9793436527252197 0.0 0.0 0.20220300555229187"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_box_3 geom"
-        mesh="collision_box_3"
-        density="10.0"
-      />
-      <body
-        name="box_3"
-        pos="-1.9073486328125e-06 -1.9073486328125e-06 0.0"
-        quat="1.0 0.0 0.0 0.0"
-      >
-        <geom
-          class="visual"
-          type="mesh"
-          name="box_3 geom"
-          mesh="box_3"
-          density="10.0"
-          material="box"
-        />
+    <body name="tire_lndg_rear_l_in.109" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.109 geom" mesh="tire_lndg_rear_l_in.109"/>
+    </body>
+    <body name="tire_lndg_rear_l_in.001" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.001 geom" mesh="tire_lndg_rear_l_in.001" material="Material.001"/>
+    </body>
+    <body name="tire_lndg_rear_l_in.002" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.002 geom" mesh="tire_lndg_rear_l_in.002"/>
+    </body>
+    <body name="tire_lndg_rear_l_in.003" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.003 geom" mesh="tire_lndg_rear_l_in.003" material="rubber"/>
+    </body>
+    <body name="tire_lndg_rear_l_in.004" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.004 geom" mesh="tire_lndg_rear_l_in.004"/>
+    </body>
+    <body name="tire_lndg_rear_l_in.005" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.005 geom" mesh="tire_lndg_rear_l_in.005"/>
+    </body>
+    <body name="tire_lndg_rear_l_in.006" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.006 geom" mesh="tire_lndg_rear_l_in.006"/>
+    </body>
+    <body name="tire_lndg_rear_l_in.007" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.007 geom" mesh="tire_lndg_rear_l_in.007" material="Material"/>
+    </body>
+    <body name="tire_lndg_rear_l_in.008" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.008 geom" mesh="tire_lndg_rear_l_in.008"/>
+    </body>
+    <body name="tire_lndg_rear_l_in.009" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.009 geom" mesh="tire_lndg_rear_l_in.009"/>
+    </body>
+    <body name="tire_lndg_rear_l_in.010" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.010 geom" mesh="tire_lndg_rear_l_in.010"/>
+    </body>
+    <body name="tire_lndg_rear_l_in.011" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.011 geom" mesh="tire_lndg_rear_l_in.011"/>
+    </body>
+    <body name="tire_lndg_rear_l_in.012" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.012 geom" mesh="tire_lndg_rear_l_in.012"/>
+    </body>
+    <body name="tire_lndg_rear_l_in.013" pos="0.003512459108605981 17.572731018066406 2.220263957977295" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="tire_lndg_rear_l_in.013 geom" mesh="tire_lndg_rear_l_in.013"/>
+    </body>
+    <body name="collision_box_3" pos="-9.765350341796875 3.763852119445801 0.1250065267086029" quat="0.9793436527252197 0.0 0.0 0.20220300555229187">
+      <geom class="collision" type="mesh" name="collision_box_3 geom" mesh="collision_box_3" density="10.0"/>
+      <body name="box_3" pos="-1.9073486328125e-06 -1.9073486328125e-06 0.0" quat="1.0 0.0 0.0 0.0">
+        <geom class="visual" type="mesh" name="box_3 geom" mesh="box_3" density="10.0" material="box"/>
       </body>
-      <joint type="free" name="box_fee_joint_1.006" pos="0.0 0.0 0.0" />
-    </body>
-    <body
-      name="collision_box_2"
-      pos="-9.425559043884277 5.182408332824707 0.1250065267086029"
-      quat="0.803748607635498 0.0 0.0 0.5949690341949463"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_box_2 geom"
-        density="10.0"
-        mesh="collision_box_2"
-      />
+      <joint type="free" name="box_fee_joint_1.006" pos="0.0 0.0 0.0"/>
+    </body>
+    <body name="collision_box_2" pos="-9.425559043884277 5.182408332824707 0.1250065267086029" quat="0.803748607635498 0.0 0.0 0.5949690341949463">
+      <geom class="collision" type="mesh" name="collision_box_2 geom" density="10.0" mesh="collision_box_2"/>
       <body name="box_2" pos="0.0 0.0 0.0" quat="1.0 0.0 0.0 0.0">
-        <geom
-          class="visual"
-          type="mesh"
-          name="box_2 geom"
-          mesh="box_2"
-          density="10.0"
-          material="box"
-        />
+        <geom class="visual" type="mesh" name="box_2 geom" mesh="box_2" density="10.0" material="box"/>
       </body>
-      <joint type="free" name="box_fee_joint_1.001" pos="0.0 0.0 0.0" />
-    </body>
-    <body
-      name="collision_box_1"
-      pos="-9.571712493896484 2.127570629119873 0.125006765127182"
-      quat="0.9880627393722534 0.0 0.0 -0.15405221283435822"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_box_1 geom"
-        mesh="collision_box_1"
-        density="10.0"
-      />
-      <body
-        name="box_1"
-        pos="-1.9073486328125e-06 -4.76837158203125e-07 0.0"
-        quat="1.0 0.0 0.0 0.0"
-      >
-        <geom
-          class="visual"
-          type="mesh"
-          name="box_1 geom"
-          mesh="box_1"
-          density="10.0"
-          material="box"
-        />
+      <joint type="free" name="box_fee_joint_1.001" pos="0.0 0.0 0.0"/>
+    </body>
+    <body name="collision_box_1" pos="-9.571712493896484 2.127570629119873 0.125006765127182" quat="0.9880627393722534 0.0 0.0 -0.15405221283435822">
+      <geom class="collision" type="mesh" name="collision_box_1 geom" mesh="collision_box_1" density="10.0"/>
+      <body name="box_1" pos="-1.9073486328125e-06 -4.76837158203125e-07 0.0" quat="1.0 0.0 0.0 0.0">
+        <geom class="visual" type="mesh" name="box_1 geom" mesh="box_1" density="10.0" material="box"/>
       </body>
-      <joint
-        type="free"
-        name="box_fee_joint_1"
-        pos="-1.9073486328125e-06 -4.76837158203125e-07 0.0"
-      />
-    </body>
-    <body
-      name="collision_box_7"
-      pos="-12.00039291381836 5.1587724685668945 0.1250065267086029"
-      quat="0.6656379699707031 0.0 0.0 0.7462748289108276"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_box_7 geom"
-        mesh="collision_box_7"
-        density="10.0"
-      />
-      <body
-        name="box_7"
-        pos="0.0 -9.5367431640625e-07 0.0"
-        quat="1.0 0.0 0.0 0.0"
-      >
-        <geom
-          class="visual"
-          type="mesh"
-          name="box_7 geom"
-          mesh="box_7"
-          density="10.0"
-          material="box"
-        />
+      <joint type="free" name="box_fee_joint_1" pos="-1.9073486328125e-06 -4.76837158203125e-07 0.0"/>
+    </body>
+    <body name="collision_box_7" pos="-12.00039291381836 5.1587724685668945 0.1250065267086029" quat="0.6656379699707031 0.0 0.0 0.7462748289108276">
+      <geom class="collision" type="mesh" name="collision_box_7 geom" mesh="collision_box_7" density="10.0"/>
+      <body name="box_7" pos="0.0 -9.5367431640625e-07 0.0" quat="1.0 0.0 0.0 0.0">
+        <geom class="visual" type="mesh" name="box_7 geom" mesh="box_7" density="10.0" material="box"/>
       </body>
-      <joint
-        type="free"
-        name="box_fee_joint_1.003"
-        pos="9.5367431640625e-07 -1.9073486328125e-06 -1.4901161193847656e-08"
-      />
-    </body>
-    <body
-      name="collision_box_5"
-      pos="-12.821616172790527 4.422228813171387 0.1250065267086029"
-      quat="0.7372055649757385 0.0 0.0 0.6756685376167297"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_box_5 geom"
-        mesh="collision_box_5"
-        density="10.0"
-      />
+      <joint type="free" name="box_fee_joint_1.003" pos="9.5367431640625e-07 -1.9073486328125e-06 -1.4901161193847656e-08"/>
+    </body>
+    <body name="collision_box_5" pos="-12.821616172790527 4.422228813171387 0.1250065267086029" quat="0.7372055649757385 0.0 0.0 0.6756685376167297">
+      <geom class="collision" type="mesh" name="collision_box_5 geom" mesh="collision_box_5" density="10.0"/>
       <body name="box_5" pos="0.0 0.0 0.0" quat="1.0 0.0 0.0 0.0">
-        <geom
-          class="visual"
-          type="mesh"
-          name="box_5 geom"
-          mesh="box_5"
-          density="10.0"
-          material="box"
-        />
+        <geom class="visual" type="mesh" name="box_5 geom" mesh="box_5" density="10.0" material="box"/>
       </body>
-      <joint type="free" name="box_fee_joint_1.004" pos="0.0 0.0 0.0" />
-    </body>
-    <body
-      name="collision_box_4"
-      pos="-11.175603866577148 5.302310943603516 0.1250065267086029"
-      quat="0.9810431003570557 0.0 0.0 -0.19378963112831116"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_box_4 geom"
-        mesh="collision_box_4"
-        density="10.0"
-      />
+      <joint type="free" name="box_fee_joint_1.004" pos="0.0 0.0 0.0"/>
+    </body>
+    <body name="collision_box_4" pos="-11.175603866577148 5.302310943603516 0.1250065267086029" quat="0.9810431003570557 0.0 0.0 -0.19378963112831116">
+      <geom class="collision" type="mesh" name="collision_box_4 geom" mesh="collision_box_4" density="10.0"/>
       <body name="box_4" pos="0.0 0.0 0.0" quat="1.0 0.0 0.0 0.0">
-        <geom
-          class="visual"
-          type="mesh"
-          name="box_4 geom"
-          mesh="box_4"
-          density="10.0"
-          material="box"
-        />
+        <geom class="visual" type="mesh" name="box_4 geom" mesh="box_4" density="10.0" material="box"/>
       </body>
-      <joint type="free" name="box_fee_joint_1.002" pos="0.0 0.0 0.0" />
-    </body>
-    <body
-      name="collision_box_8"
-      pos="-8.844049453735352 3.162076950073242 0.1250065267086029"
-      quat="0.9648564457893372 0.0 0.0 0.26277756690979004"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_box_8 geom"
-        mesh="collision_box_8"
-        density="10.0"
-      />
-      <body
-        name="box_8"
-        pos="-5.245208740234375e-06 4.76837158203125e-07 -1.4901161193847656e-08"
-        quat="1.0 0.0 0.0 0.0"
-      >
-        <geom
-          class="visual"
-          type="mesh"
-          name="box_8 geom"
-          mesh="box_8"
-          density="10.0"
-          material="box"
-        />
+      <joint type="free" name="box_fee_joint_1.002" pos="0.0 0.0 0.0"/>
+    </body>
+    <body name="collision_box_8" pos="-8.844049453735352 3.162076950073242 0.1250065267086029" quat="0.9648564457893372 0.0 0.0 0.26277756690979004">
+      <geom class="collision" type="mesh" name="collision_box_8 geom" mesh="collision_box_8" density="10.0"/>
+      <body name="box_8" pos="-5.245208740234375e-06 4.76837158203125e-07 -1.4901161193847656e-08" quat="1.0 0.0 0.0 0.0">
+        <geom class="visual" type="mesh" name="box_8 geom" mesh="box_8" density="10.0" material="box"/>
       </body>
-      <joint
-        type="free"
-        name="box_fee_joint_1.005"
-        pos="-3.814697265625e-06 -9.5367431640625e-07 0.0"
-      />
-    </body>
-    <body
-      name="collision_box_6"
-      pos="-10.643143653869629 1.4997047185897827 0.1250065267086029"
-      quat="0.8876487016677856 0.0 0.0 0.4605211913585663"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_box_6 geom"
-        mesh="collision_box_6"
-        density="10.0"
-      />
-      <body
-        name="box_6"
-        pos="1.430511474609375e-06 -9.5367431640625e-07 -1.4901161193847656e-08"
-        quat="1.0 0.0 0.0 0.0"
-      >
-        <geom
-          class="visual"
-          type="mesh"
-          name="box_6 geom"
-          mesh="box_6"
-          density="10.0"
-          material="box"
-        />
+      <joint type="free" name="box_fee_joint_1.005" pos="-3.814697265625e-06 -9.5367431640625e-07 0.0"/>
+    </body>
+    <body name="collision_box_6" pos="-10.643143653869629 1.4997047185897827 0.1250065267086029" quat="0.8876487016677856 0.0 0.0 0.4605211913585663">
+      <geom class="collision" type="mesh" name="collision_box_6 geom" mesh="collision_box_6" density="10.0"/>
+      <body name="box_6" pos="1.430511474609375e-06 -9.5367431640625e-07 -1.4901161193847656e-08" quat="1.0 0.0 0.0 0.0">
+        <geom class="visual" type="mesh" name="box_6 geom" mesh="box_6" density="10.0" material="box"/>
       </body>
-      <joint
-        type="free"
-        name="box_fee_joint_1.007"
-        pos="1.9073486328125e-06 -9.5367431640625e-07 -2.9802322387695312e-08"
-      />
+      <joint type="free" name="box_fee_joint_1.007" pos="1.9073486328125e-06 -9.5367431640625e-07 -2.9802322387695312e-08"/>
     </body>
   </worldbody>
   <asset>
-    <mesh name="SM_WallSide4" file="hangar_mesh/SM_WallSide4.obj" />
-    <mesh name="SM_WallSide16" file="hangar_mesh/SM_WallSide16.obj" />
-    <mesh name="SM_Roof4" file="hangar_mesh/SM_Roof4.obj" />
-    <mesh name="SM_Roof16_360" file="hangar_mesh/SM_Roof16_360.obj" />
-    <mesh name="SM_Floor_376" file="hangar_mesh/SM_Floor_376.obj" />
-    <mesh name="SM_WallFront_382" file="hangar_mesh/SM_WallFront_382.obj" />
-    <mesh name="SM_WallBack_396" file="hangar_mesh/SM_WallBack_396.obj" />
-    <mesh name="SM_DoorSlide_399" file="hangar_mesh/SM_DoorSlide_399.obj" />
-    <mesh name="SM_WallSupport" file="hangar_mesh/SM_WallSupport.obj" />
-    <mesh name="SM_WallSupport2" file="hangar_mesh/SM_WallSupport2.obj" />
-    <mesh name="SM_WallSupport3" file="hangar_mesh/SM_WallSupport3.obj" />
-    <mesh name="SM_WallSupport4" file="hangar_mesh/SM_WallSupport4.obj" />
-    <mesh name="SM_WallSupport5" file="hangar_mesh/SM_WallSupport5.obj" />
-    <mesh name="SM_WallSupport6" file="hangar_mesh/SM_WallSupport6.obj" />
-    <mesh name="SM_WallSupport7" file="hangar_mesh/SM_WallSupport7.obj" />
-    <mesh name="SM_WallSupport8" file="hangar_mesh/SM_WallSupport8.obj" />
-    <mesh name="SM_WallSupport9" file="hangar_mesh/SM_WallSupport9.obj" />
-    <mesh name="SM_WallSupport10" file="hangar_mesh/SM_WallSupport10.obj" />
-    <mesh name="SM_WallSupport11" file="hangar_mesh/SM_WallSupport11.obj" />
-    <mesh name="SM_WallSupport12" file="hangar_mesh/SM_WallSupport12.obj" />
-    <mesh name="SM_WallSupport13" file="hangar_mesh/SM_WallSupport13.obj" />
-    <mesh name="SM_WallSupport14" file="hangar_mesh/SM_WallSupport14.obj" />
-    <mesh name="SM_Room_426" file="hangar_mesh/SM_Room_426.obj" />
-    <mesh name="SM_DoorSingle_430" file="hangar_mesh/SM_DoorSingle_430.obj" />
-    <mesh name="SM_DoorSingle2" file="hangar_mesh/SM_DoorSingle2.obj" />
-    <mesh name="SM_AccessStand_441" file="hangar_mesh/SM_AccessStand_441.obj" />
-    <mesh
-      name="collision_Cube_002.001"
-      file="hangar_mesh/collision_Cube_002.001.obj"
-    />
-    <mesh
-      name="SM_AccessStand_441.001"
-      file="hangar_mesh/SM_AccessStand_441.001.obj"
-    />
-    <mesh name="SM_Lamps6" file="hangar_mesh/SM_Lamps6.obj" />
-    <mesh name="SM_TrashBin_22" file="hangar_mesh/SM_TrashBin_22.obj" />
-    <mesh name="SM_Cart_28" file="hangar_mesh/SM_Cart_28.obj" />
-    <mesh name="SM_Ladder_31" file="hangar_mesh/SM_Ladder_31.obj" />
-    <mesh name="collision_Cube_004" file="hangar_mesh/collision_Cube_004.obj" />
-    <mesh name="SM_Shelf_A2" file="hangar_mesh/SM_Shelf_A2.obj" />
-    <mesh name="SM_Shelf_A3" file="hangar_mesh/SM_Shelf_A3.obj" />
-    <mesh name="SM_Shelf_B_39" file="hangar_mesh/SM_Shelf_B_39.obj" />
-    <mesh name="SM_Shelf_B2" file="hangar_mesh/SM_Shelf_B2.obj" />
-    <mesh name="SM_Shelf_B3" file="hangar_mesh/SM_Shelf_B3.obj" />
-    <mesh name="SM_Box_A_44" file="hangar_mesh/SM_Box_A_44.obj" />
-    <mesh name="SM_Box_A2" file="hangar_mesh/SM_Box_A2.obj" />
-    <mesh name="SM_Box_A3" file="hangar_mesh/SM_Box_A3.obj" />
-    <mesh name="SM_Box_A4" file="hangar_mesh/SM_Box_A4.obj" />
-    <mesh name="SM_Box_A5" file="hangar_mesh/SM_Box_A5.obj" />
-    <mesh name="SM_Box_A6" file="hangar_mesh/SM_Box_A6.obj" />
-    <mesh name="SM_Box_B_54" file="hangar_mesh/SM_Box_B_54.obj" />
-    <mesh name="SM_Box_B2" file="hangar_mesh/SM_Box_B2.obj" />
-    <mesh name="SM_Box_B3" file="hangar_mesh/SM_Box_B3.obj" />
-    <mesh name="SM_Box_B4" file="hangar_mesh/SM_Box_B4.obj" />
-    <mesh name="SM_Box_B5" file="hangar_mesh/SM_Box_B5.obj" />
-    <mesh name="SM_Box_B6" file="hangar_mesh/SM_Box_B6.obj" />
-    <mesh name="SM_Box_B7" file="hangar_mesh/SM_Box_B7.obj" />
-    <mesh name="SM_Box_C_63" file="hangar_mesh/SM_Box_C_63.obj" />
-    <mesh name="SM_Box_C2" file="hangar_mesh/SM_Box_C2.obj" />
-    <mesh name="SM_Box_C3" file="hangar_mesh/SM_Box_C3.obj" />
-    <mesh name="SM_Box_C4_68" file="hangar_mesh/SM_Box_C4_68.obj" />
-    <mesh name="SM_Box_C5" file="hangar_mesh/SM_Box_C5.obj" />
-    <mesh name="SM_Box_C6" file="hangar_mesh/SM_Box_C6.obj" />
-    <mesh
-      name="collision_SM_Box_A7_73"
-      file="hangar_mesh/collision_SM_Box_A7_73.obj"
-    />
-    <mesh
-      name="collision_SM_Box_A8"
-      file="hangar_mesh/collision_SM_Box_A8.obj"
-    />
-    <mesh
-      name="collision_SM_Box_C7_77"
-      file="hangar_mesh/collision_SM_Box_C7_77.obj"
-    />
-    <mesh name="SM_Room2_3" file="hangar_mesh/SM_Room2_3.obj" />
-    <mesh name="SM_Pillar_10" file="hangar_mesh/SM_Pillar_10.obj" />
-    <mesh name="SM_Pillar2" file="hangar_mesh/SM_Pillar2.obj" />
-    <mesh name="SM_Pillar3" file="hangar_mesh/SM_Pillar3.obj" />
-    <mesh name="SM_Pillar4" file="hangar_mesh/SM_Pillar4.obj" />
-    <mesh name="SM_Pillar5" file="hangar_mesh/SM_Pillar5.obj" />
-    <mesh name="SM_Pillar6" file="hangar_mesh/SM_Pillar6.obj" />
-    <mesh name="SM_Pillar7" file="hangar_mesh/SM_Pillar7.obj" />
-    <mesh name="SM_Pillar8" file="hangar_mesh/SM_Pillar8.obj" />
-    <mesh name="SM_DoorSingle3" file="hangar_mesh/SM_DoorSingle3.obj" />
-    <mesh name="SM_DoorDouble_26" file="hangar_mesh/SM_DoorDouble_26.obj" />
-    <mesh name="SM_DoorDouble2" file="hangar_mesh/SM_DoorDouble2.obj" />
-    <mesh name="SM_DoorSingle4" file="hangar_mesh/SM_DoorSingle4.obj" />
-    <mesh name="SM_Stairs_32" file="hangar_mesh/SM_Stairs_32.obj" />
-    <mesh name="SM_Stairs2" file="hangar_mesh/SM_Stairs2.obj" />
-    <mesh
-      name="SM_StairsRailing_Inner_2"
-      file="hangar_mesh/SM_StairsRailing_Inner_2.obj"
-    />
-    <mesh
-      name="SM_StairsRailing_Outer_7"
-      file="hangar_mesh/SM_StairsRailing_Outer_7.obj"
-    />
-    <mesh
-      name="SM_StairsRailing_Top_10"
-      file="hangar_mesh/SM_StairsRailing_Top_10.obj"
-    />
-    <mesh
-      name="SM_StairsRailing_Outer2"
-      file="hangar_mesh/SM_StairsRailing_Outer2.obj"
-    />
-    <mesh
-      name="SM_StairsRailing_Inner2"
-      file="hangar_mesh/SM_StairsRailing_Inner2.obj"
-    />
-    <mesh
-      name="SM_StairsRailing_Top2_15"
-      file="hangar_mesh/SM_StairsRailing_Top2_15.obj"
-    />
-    <mesh name="SM_DoorSingle5" file="hangar_mesh/SM_DoorSingle5.obj" />
-    <mesh name="SM_DoorSingle6" file="hangar_mesh/SM_DoorSingle6.obj" />
-    <mesh name="SM_WallRailing_H_2" file="hangar_mesh/SM_WallRailing_H_2.obj" />
-    <mesh
-      name="SM_WallRailing_H2_5"
-      file="hangar_mesh/SM_WallRailing_H2_5.obj"
-    />
-    <mesh name="SM_WallRailing_H3" file="hangar_mesh/SM_WallRailing_H3.obj" />
-    <mesh name="SM_WallRailing_H4" file="hangar_mesh/SM_WallRailing_H4.obj" />
-    <mesh name="SM_WallRailing_H5" file="hangar_mesh/SM_WallRailing_H5.obj" />
-    <mesh name="SM_WallRailing_H6" file="hangar_mesh/SM_WallRailing_H6.obj" />
-    <mesh name="SM_WallRailing_H7" file="hangar_mesh/SM_WallRailing_H7.obj" />
-    <mesh name="SM_WallRailing_H8" file="hangar_mesh/SM_WallRailing_H8.obj" />
-    <mesh name="SM_WallRailing_H9" file="hangar_mesh/SM_WallRailing_H9.obj" />
-    <mesh name="SM_WallRailing_H10" file="hangar_mesh/SM_WallRailing_H10.obj" />
-    <mesh name="SM_WallRailing_H11" file="hangar_mesh/SM_WallRailing_H11.obj" />
-    <mesh name="SM_WallRailing_H12" file="hangar_mesh/SM_WallRailing_H12.obj" />
-    <mesh name="SM_WallRailing_H13" file="hangar_mesh/SM_WallRailing_H13.obj" />
-    <mesh name="SM_WallRailing_H14" file="hangar_mesh/SM_WallRailing_H14.obj" />
-    <mesh name="SM_WallRailing_H15" file="hangar_mesh/SM_WallRailing_H15.obj" />
-    <mesh name="SM_WallRailing_H16" file="hangar_mesh/SM_WallRailing_H16.obj" />
-    <mesh name="SM_WallRailing_H17" file="hangar_mesh/SM_WallRailing_H17.obj" />
-    <mesh name="SM_WallRailing_H18" file="hangar_mesh/SM_WallRailing_H18.obj" />
-    <mesh name="SM_WallRailing_H19" file="hangar_mesh/SM_WallRailing_H19.obj" />
-    <mesh name="SM_DoorDouble3" file="hangar_mesh/SM_DoorDouble3.obj" />
-    <mesh name="SM_AccessStand2" file="hangar_mesh/SM_AccessStand2.obj" />
-    <mesh name="SM_Ladder2" file="hangar_mesh/SM_Ladder2.obj" />
-    <mesh name="SM_Ladder3" file="hangar_mesh/SM_Ladder3.obj" />
-    <mesh name="SM_TrashBin2" file="hangar_mesh/SM_TrashBin2.obj" />
-    <mesh name="SM_Box_C8" file="hangar_mesh/SM_Box_C8.obj" />
-    <mesh name="SM_Box_C9" file="hangar_mesh/SM_Box_C9.obj" />
-    <mesh name="SM_Box_C10" file="hangar_mesh/SM_Box_C10.obj" />
-    <mesh name="SM_Box_B8" file="hangar_mesh/SM_Box_B8.obj" />
-    <mesh
-      name="SM_Railing_Corner6_20_GEN_VARIABLE_StaticMeshActor_CAT_486"
-      file="hangar_mesh/SM_Railing_Corner6_20_GEN_VARIABLE_StaticMeshActor_CAT_486.obj"
-    />
-    <mesh
-      name="SM_DockingStation_Stairs2_GEN_VARIABLE_StaticMeshActor_CAT_7726"
-      file="hangar_mesh/SM_DockingStation_Stairs2_GEN_VARIABLE_StaticMeshActor_CAT_7726.obj"
-    />
-    <mesh
-      name="SM_DockingStation_Top_50_GEN_VARIABLE_StaticMeshActor_CAT_7711"
-      file="hangar_mesh/SM_DockingStation_Top_50_GEN_VARIABLE_StaticMeshActor_CAT_7711.obj"
-    />
-    <mesh
-      name="SM_DockingStation_Stairs_46_GEN_VARIABLE_StaticMeshActo_37a21c2"
-      file="hangar_mesh/SM_DockingStation_Stairs_46_GEN_VARIABLE_StaticMeshActo_37a21c2.obj"
-    />
-    <mesh
-      name="SM_DockingStation_438_GEN_VARIABLE_StaticMeshActor_CAT_7694"
-      file="hangar_mesh/SM_DockingStation_438_GEN_VARIABLE_StaticMeshActor_CAT_7694.obj"
-    />
-    <mesh name="SM_Cart2" file="hangar_mesh/SM_Cart2.obj" />
-    <mesh name="SM_TrashBin4" file="hangar_mesh/SM_TrashBin4.obj" />
-    <mesh
-      name="SM_WallFront_382.003"
-      file="hangar_mesh/SM_WallFront_382.003.obj"
-    />
-    <mesh
-      name="SM_WallFront_382.00311"
-      file="hangar_mesh/SM_WallFront_382.00311.obj"
-    />
-    <mesh
-      name="SM_DoorSlide_399.001"
-      file="hangar_mesh/SM_DoorSlide_399.001.obj"
-    />
-    <mesh name="SM_WallSide16.001" file="hangar_mesh/SM_WallSide16.001.obj" />
-    <mesh name="SM_WallSide16.002" file="hangar_mesh/SM_WallSide16.002.obj" />
-    <mesh
-      name="collision_SM_WallSide4.001"
-      file="hangar_mesh/collision_SM_WallSide4.001.obj"
-    />
-    <mesh name="SM_WallSide4.002" file="hangar_mesh/SM_WallSide4.002.obj" />
-    <mesh name="SM_Lamps6.001" file="hangar_mesh/SM_Lamps6.001.obj" />
-    <mesh
-      name="SM_WallBack_396.001"
-      file="hangar_mesh/SM_WallBack_396.001.obj"
-    />
-    <mesh name="SM_Roof16_360.001" file="hangar_mesh/SM_Roof16_360.001.obj" />
-    <mesh name="SM_Roof4.001" file="hangar_mesh/SM_Roof4.001.obj" />
-    <mesh name="Cube.003" file="hangar_mesh/Cube.003.obj" />
-    <mesh name="collision_Plane" file="hangar_mesh/collision_Plane.obj" />
-    <mesh
-      name="collision_Plane.001"
-      file="hangar_mesh/collision_Plane.001.obj"
-    />
-    <mesh
-      name="collision_Plane.002"
-      file="hangar_mesh/collision_Plane.002.obj"
-    />
-    <mesh
-      name="collision_Plane.003"
-      file="hangar_mesh/collision_Plane.003.obj"
-    />
-    <mesh
-      name="collision_Plane.004"
-      file="hangar_mesh/collision_Plane.004.obj"
-    />
-    <mesh
-      name="collision_Plane.005"
-      file="hangar_mesh/collision_Plane.005.obj"
-    />
-    <mesh
-      name="collision_Plane.006"
-      file="hangar_mesh/collision_Plane.006.obj"
-    />
-    <mesh
-      name="collision_Plane.007"
-      file="hangar_mesh/collision_Plane.007.obj"
-    />
-    <mesh
-      name="collision_Plane.008"
-      file="hangar_mesh/collision_Plane.008.obj"
-    />
-    <mesh
-      name="collision_Plane.009"
-      file="hangar_mesh/collision_Plane.009.obj"
-    />
-    <mesh name="collision_Cube" file="hangar_mesh/collision_Cube.obj" />
-    <mesh name="collision_Cube.001" file="hangar_mesh/collision_Cube.001.obj" />
-    <mesh name="collision_Cube.002" file="hangar_mesh/collision_Cube.002.obj" />
-    <mesh name="collision_Cube.004" file="hangar_mesh/collision_Cube.004.obj" />
-    <mesh name="collision_Cube.005" file="hangar_mesh/collision_Cube.005.obj" />
-    <mesh name="collision_Cube.006" file="hangar_mesh/collision_Cube.006.obj" />
-    <mesh name="collision_Cube.007" file="hangar_mesh/collision_Cube.007.obj" />
-    <mesh name="collision_Cube.008" file="hangar_mesh/collision_Cube.008.obj" />
-    <mesh name="collision_Cube.009" file="hangar_mesh/collision_Cube.009.obj" />
-    <mesh name="collision_Cube.010" file="hangar_mesh/collision_Cube.010.obj" />
-    <mesh name="collision_Cube.011" file="hangar_mesh/collision_Cube.011.obj" />
-    <mesh name="collision_Cube.012" file="hangar_mesh/collision_Cube.012.obj" />
-    <mesh name="collision_Cube.013" file="hangar_mesh/collision_Cube.013.obj" />
-    <mesh name="collision_Cube.014" file="hangar_mesh/collision_Cube.014.obj" />
-    <mesh name="collision_Cube.015" file="hangar_mesh/collision_Cube.015.obj" />
-    <mesh
-      name="collision_SM_Room_426.001"
-      file="hangar_mesh/collision_SM_Room_426.001.obj"
-    />
-    <mesh name="SM_Room_426.002" file="hangar_mesh/SM_Room_426.002.obj" />
-    <mesh
-      name="collision_Plane.010"
-      file="hangar_mesh/collision_Plane.010.obj"
-    />
-    <mesh
-      name="collision_Plane.011"
-      file="hangar_mesh/collision_Plane.011.obj"
-    />
-    <mesh
-      name="collision_Plane.012"
-      file="hangar_mesh/collision_Plane.012.obj"
-    />
-    <mesh name="collision_supports" file="hangar_mesh/collision_supports.obj" />
-    <mesh
-      name="collision_supports.001"
-      file="hangar_mesh/collision_supports.001.obj"
-    />
-    <mesh
-      name="collision_supports.002"
-      file="hangar_mesh/collision_supports.002.obj"
-    />
-    <mesh
-      name="collision_supports.003"
-      file="hangar_mesh/collision_supports.003.obj"
-    />
-    <mesh
-      name="collision_supports.004"
-      file="hangar_mesh/collision_supports.004.obj"
-    />
-    <mesh
-      name="collision_supports.005"
-      file="hangar_mesh/collision_supports.005.obj"
-    />
-    <mesh
-      name="collision_supports.006"
-      file="hangar_mesh/collision_supports.006.obj"
-    />
-    <mesh
-      name="collision_supports.007"
-      file="hangar_mesh/collision_supports.007.obj"
-    />
-    <mesh
-      name="collision_supports.008"
-      file="hangar_mesh/collision_supports.008.obj"
-    />
-    <mesh
-      name="collision_supports.009"
-      file="hangar_mesh/collision_supports.009.obj"
-    />
-    <mesh
-      name="collision_supports.010"
-      file="hangar_mesh/collision_supports.010.obj"
-    />
-    <mesh
-      name="collision_supports.011"
-      file="hangar_mesh/collision_supports.011.obj"
-    />
-    <mesh
-      name="collision_supports.012"
-      file="hangar_mesh/collision_supports.012.obj"
-    />
-    <mesh
-      name="collision_supports.013"
-      file="hangar_mesh/collision_supports.013.obj"
-    />
-    <mesh
-      name="collision_supports.014"
-      file="hangar_mesh/collision_supports.014.obj"
-    />
-    <mesh
-      name="collision_supports.015"
-      file="hangar_mesh/collision_supports.015.obj"
-    />
-    <mesh
-      name="SM_WallBack_396.002"
-      file="hangar_mesh/SM_WallBack_396.002.obj"
-    />
-    <mesh
-      name="SM_WallBack_396.003"
-      file="hangar_mesh/SM_WallBack_396.003.obj"
-    />
-    <mesh
-      name="SM_WallFront_382.002"
-      file="hangar_mesh/SM_WallFront_382.002.obj"
-    />
-    <mesh
-      name="collision_SM_WallFront_382.003"
-      file="hangar_mesh/collision_SM_WallFront_382.003.obj"
-    />
-    <mesh
-      name="collision_SM_Room_426.003"
-      file="hangar_mesh/collision_SM_Room_426.003.obj"
-    />
-    <mesh
-      name="collision_SM_Room_426.004"
-      file="hangar_mesh/collision_SM_Room_426.004.obj"
-    />
-    <mesh
-      name="collision_SM_Floor_376"
-      file="hangar_mesh/collision_SM_Floor_376.obj"
-    />
-    <mesh name="SM_WallSide4.001" file="hangar_mesh/SM_WallSide4.001.obj" />
-    <mesh
-      name="collision_SM_WallBack_396.001"
-      file="hangar_mesh/collision_SM_WallBack_396.001.obj"
-    />
-    <mesh
-      name="collision_SM_WallBack_396.002"
-      file="hangar_mesh/collision_SM_WallBack_396.002.obj"
-    />
-    <mesh
-      name="collision_SM_WallSide16.002"
-      file="hangar_mesh/collision_SM_WallSide16.002.obj"
-    />
-    <mesh
-      name="collision_SM_WallFront_382.001"
-      file="hangar_mesh/collision_SM_WallFront_382.001.obj"
-    />
-    <mesh
-      name="collision_SM_WallFront_382.002"
-      file="hangar_mesh/collision_SM_WallFront_382.002.obj"
-    />
-    <mesh name="SM_Box_A7_73.001" file="hangar_mesh/SM_Box_A7_73.001.obj" />
-    <mesh name="SM_Box_A8.001" file="hangar_mesh/SM_Box_A8.001.obj" />
-    <mesh name="SM_Box_C7_77.001" file="hangar_mesh/SM_Box_C7_77.001.obj" />
-    <mesh
-      name="collision_SM_Pillar4.001"
-      file="hangar_mesh/collision_SM_Pillar4.001.obj"
-    />
-    <mesh
-      name="collision_SM_Pillar3.001"
-      file="hangar_mesh/collision_SM_Pillar3.001.obj"
-    />
-    <mesh
-      name="collision_SM_Pillar2.001"
-      file="hangar_mesh/collision_SM_Pillar2.001.obj"
-    />
-    <mesh
-      name="collision_SM_Pillar_10.001"
-      file="hangar_mesh/collision_SM_Pillar_10.001.obj"
-    />
-    <mesh
-      name="collision_SM_Pillar8.001"
-      file="hangar_mesh/collision_SM_Pillar8.001.obj"
-    />
-    <mesh
-      name="collision_SM_Pillar7.001"
-      file="hangar_mesh/collision_SM_Pillar7.001.obj"
-    />
-    <mesh
-      name="collision_SM_Pillar6.001"
-      file="hangar_mesh/collision_SM_Pillar6.001.obj"
-    />
-    <mesh
-      name="collision_SM_Pillar5.001"
-      file="hangar_mesh/collision_SM_Pillar5.001.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.109"
-      file="hangar_mesh/tire_lndg_rear_l_in.109.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.001"
-      file="hangar_mesh/tire_lndg_rear_l_in.001.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.002"
-      file="hangar_mesh/tire_lndg_rear_l_in.002.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.003"
-      file="hangar_mesh/tire_lndg_rear_l_in.003.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.004"
-      file="hangar_mesh/tire_lndg_rear_l_in.004.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.005"
-      file="hangar_mesh/tire_lndg_rear_l_in.005.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.006"
-      file="hangar_mesh/tire_lndg_rear_l_in.006.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.007"
-      file="hangar_mesh/tire_lndg_rear_l_in.007.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.008"
-      file="hangar_mesh/tire_lndg_rear_l_in.008.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.009"
-      file="hangar_mesh/tire_lndg_rear_l_in.009.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.010"
-      file="hangar_mesh/tire_lndg_rear_l_in.010.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.011"
-      file="hangar_mesh/tire_lndg_rear_l_in.011.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.012"
-      file="hangar_mesh/tire_lndg_rear_l_in.012.obj"
-    />
-    <mesh
-      name="tire_lndg_rear_l_in.013"
-      file="hangar_mesh/tire_lndg_rear_l_in.013.obj"
-    />
-    <mesh name="collision_box_1" file="hangar_mesh/collision_box_1.obj" />
-    <mesh name="collision_box_2" file="hangar_mesh/collision_box_2.obj" />
-    <mesh name="collision_box_3" file="hangar_mesh/collision_box_3.obj" />
-    <mesh name="collision_box_4" file="hangar_mesh/collision_box_4.obj" />
-    <mesh name="collision_box_5" file="hangar_mesh/collision_box_5.obj" />
-    <mesh name="collision_box_6" file="hangar_mesh/collision_box_6.obj" />
-    <mesh name="collision_box_7" file="hangar_mesh/collision_box_7.obj" />
-    <mesh name="collision_box_8" file="hangar_mesh/collision_box_8.obj" />
-    <mesh name="box_1" file="hangar_mesh/box_1.obj" />
-    <mesh name="box_2" file="hangar_mesh/box_2.obj" />
-    <mesh name="box_3" file="hangar_mesh/box_3.obj" />
-    <mesh name="box_4" file="hangar_mesh/box_4.obj" />
-    <mesh name="box_5" file="hangar_mesh/box_5.obj" />
-    <mesh name="box_6" file="hangar_mesh/box_6.obj" />
-    <mesh name="box_7" file="hangar_mesh/box_7.obj" />
-    <mesh name="box_8" file="hangar_mesh/box_8.obj" />
-    <material
-      name="metal_hangar"
-      rgba="0.25 0.25 0.25 1.0"
-      reflectance="0.8960324823856354"
-      metallic="0.7795275449752808"
-    />
-    <material
-      name="glass"
-      rgba="0.8258873820304871 0.9459500908851624 0.9999716877937317 0.28333336114883423"
-      reflectance="0.9740158449858427"
-      metallic="0.0"
-    />
-    <texture
-      type="2d"
-      name="floor"
-      file="assets/floor.png"
-      mark="edge"
-      width="2048"
-      height="2048"
-    />
-    <material
-      name="floor"
-      texture="floor"
-      texuniform="true"
-      texrepeat="1 1"
-      reflectance="0.4472135901451111"
-      metallic="0.0"
-    />
-    <texture
-      type="2d"
-      name="walls"
-      file="assets/walls.png"
-      mark="edge"
-      width="2048"
-      height="2048"
-    />
-    <material
-      name="walls"
-      texture="walls"
-      texuniform="true"
-      texrepeat="1 1"
-      reflectance="0.4472135901451111"
-      metallic="0.0"
-    />
-    <material
-      name="yellow metal"
-      rgba="1.0 0.6478113532066345 0.015450038015842438 1.0"
-      reflectance="0.880284458398819"
-      metallic="0.8976377844810486"
-    />
-    <material
-      name="light"
-      rgba="0.9999716877937317 0.9999716877937317 0.9999716877937317 1.0"
-      reflectance="0.5"
-      metallic="0.0"
-    />
-    <material
-      name="box"
-      rgba="0.7859447598457336 0.521110475063324 0.2919864058494568 1.0"
-      reflectance="0.4472135901451111"
-      metallic="0.0"
-    />
-    <material
-      name="yellow_tape"
-      rgba="0.8001177906990051 0.6553049087524414 0.0 1.0"
-      reflectance="0.18644070625305176"
-      metallic="0.0"
-    />
-    <material
-      name="Material.001"
-      rgba="0.9999716877937317 0.9999716877937317 0.9999716877937317 1.0"
-      reflectance="0.5"
-      metallic="0.0"
-    />
-    <material
-      name="rubber"
-      rgba="0.01345729734748602 0.01345729734748602 0.01345729734748602 1.0"
-      reflectance="0.5"
-      metallic="0.0"
-    />
-    <material
-      name="Material"
-      rgba="0.1858992576599121 0.1858992576599121 0.1858992576599121 1.0"
-      reflectance="1.0"
-      metallic="0.06779661029577255"
-    />
+    <mesh name="SM_WallSide4" file="hangar_mesh/SM_WallSide4.obj"/>
+    <mesh name="SM_WallSide16" file="hangar_mesh/SM_WallSide16.obj"/>
+    <mesh name="SM_Roof4" file="hangar_mesh/SM_Roof4.obj"/>
+    <mesh name="SM_Roof16_360" file="hangar_mesh/SM_Roof16_360.obj"/>
+    <mesh name="SM_Floor_376" file="hangar_mesh/SM_Floor_376.obj"/>
+    <mesh name="SM_WallFront_382" file="hangar_mesh/SM_WallFront_382.obj"/>
+    <mesh name="SM_WallBack_396" file="hangar_mesh/SM_WallBack_396.obj"/>
+    <mesh name="SM_DoorSlide_399" file="hangar_mesh/SM_DoorSlide_399.obj"/>
+    <mesh name="SM_WallSupport" file="hangar_mesh/SM_WallSupport.obj"/>
+    <mesh name="SM_WallSupport2" file="hangar_mesh/SM_WallSupport2.obj"/>
+    <mesh name="SM_WallSupport3" file="hangar_mesh/SM_WallSupport3.obj"/>
+    <mesh name="SM_WallSupport4" file="hangar_mesh/SM_WallSupport4.obj"/>
+    <mesh name="SM_WallSupport5" file="hangar_mesh/SM_WallSupport5.obj"/>
+    <mesh name="SM_WallSupport6" file="hangar_mesh/SM_WallSupport6.obj"/>
+    <mesh name="SM_WallSupport7" file="hangar_mesh/SM_WallSupport7.obj"/>
+    <mesh name="SM_WallSupport8" file="hangar_mesh/SM_WallSupport8.obj"/>
+    <mesh name="SM_WallSupport9" file="hangar_mesh/SM_WallSupport9.obj"/>
+    <mesh name="SM_WallSupport10" file="hangar_mesh/SM_WallSupport10.obj"/>
+    <mesh name="SM_WallSupport11" file="hangar_mesh/SM_WallSupport11.obj"/>
+    <mesh name="SM_WallSupport12" file="hangar_mesh/SM_WallSupport12.obj"/>
+    <mesh name="SM_WallSupport13" file="hangar_mesh/SM_WallSupport13.obj"/>
+    <mesh name="SM_WallSupport14" file="hangar_mesh/SM_WallSupport14.obj"/>
+    <mesh name="SM_Room_426" file="hangar_mesh/SM_Room_426.obj"/>
+    <mesh name="SM_DoorSingle_430" file="hangar_mesh/SM_DoorSingle_430.obj"/>
+    <mesh name="SM_DoorSingle2" file="hangar_mesh/SM_DoorSingle2.obj"/>
+    <mesh name="SM_AccessStand_441" file="hangar_mesh/SM_AccessStand_441.obj"/>
+    <mesh name="collision_Cube_002.001" file="hangar_mesh/collision_Cube_002.001.obj"/>
+    <mesh name="SM_AccessStand_441.001" file="hangar_mesh/SM_AccessStand_441.001.obj"/>
+    <mesh name="SM_Lamps6" file="hangar_mesh/SM_Lamps6.obj"/>
+    <mesh name="SM_TrashBin_22" file="hangar_mesh/SM_TrashBin_22.obj"/>
+    <mesh name="SM_Cart_28" file="hangar_mesh/SM_Cart_28.obj"/>
+    <mesh name="SM_Ladder_31" file="hangar_mesh/SM_Ladder_31.obj"/>
+    <mesh name="collision_Cube_004" file="hangar_mesh/collision_Cube_004.obj"/>
+    <mesh name="SM_Shelf_A2" file="hangar_mesh/SM_Shelf_A2.obj"/>
+    <mesh name="SM_Shelf_A3" file="hangar_mesh/SM_Shelf_A3.obj"/>
+    <mesh name="SM_Shelf_B_39" file="hangar_mesh/SM_Shelf_B_39.obj"/>
+    <mesh name="SM_Shelf_B2" file="hangar_mesh/SM_Shelf_B2.obj"/>
+    <mesh name="SM_Shelf_B3" file="hangar_mesh/SM_Shelf_B3.obj"/>
+    <mesh name="SM_Box_A_44" file="hangar_mesh/SM_Box_A_44.obj"/>
+    <mesh name="SM_Box_A2" file="hangar_mesh/SM_Box_A2.obj"/>
+    <mesh name="SM_Box_A3" file="hangar_mesh/SM_Box_A3.obj"/>
+    <mesh name="SM_Box_A4" file="hangar_mesh/SM_Box_A4.obj"/>
+    <mesh name="SM_Box_A5" file="hangar_mesh/SM_Box_A5.obj"/>
+    <mesh name="SM_Box_A6" file="hangar_mesh/SM_Box_A6.obj"/>
+    <mesh name="SM_Box_B_54" file="hangar_mesh/SM_Box_B_54.obj"/>
+    <mesh name="SM_Box_B2" file="hangar_mesh/SM_Box_B2.obj"/>
+    <mesh name="SM_Box_B3" file="hangar_mesh/SM_Box_B3.obj"/>
+    <mesh name="SM_Box_B4" file="hangar_mesh/SM_Box_B4.obj"/>
+    <mesh name="SM_Box_B5" file="hangar_mesh/SM_Box_B5.obj"/>
+    <mesh name="SM_Box_B6" file="hangar_mesh/SM_Box_B6.obj"/>
+    <mesh name="SM_Box_B7" file="hangar_mesh/SM_Box_B7.obj"/>
+    <mesh name="SM_Box_C_63" file="hangar_mesh/SM_Box_C_63.obj"/>
+    <mesh name="SM_Box_C2" file="hangar_mesh/SM_Box_C2.obj"/>
+    <mesh name="SM_Box_C3" file="hangar_mesh/SM_Box_C3.obj"/>
+    <mesh name="SM_Box_C4_68" file="hangar_mesh/SM_Box_C4_68.obj"/>
+    <mesh name="SM_Box_C5" file="hangar_mesh/SM_Box_C5.obj"/>
+    <mesh name="SM_Box_C6" file="hangar_mesh/SM_Box_C6.obj"/>
+    <mesh name="collision_SM_Box_A7_73" file="hangar_mesh/collision_SM_Box_A7_73.obj"/>
+    <mesh name="collision_SM_Box_A8" file="hangar_mesh/collision_SM_Box_A8.obj"/>
+    <mesh name="collision_SM_Box_C7_77" file="hangar_mesh/collision_SM_Box_C7_77.obj"/>
+    <mesh name="SM_Room2_3" file="hangar_mesh/SM_Room2_3.obj"/>
+    <mesh name="SM_Pillar_10" file="hangar_mesh/SM_Pillar_10.obj"/>
+    <mesh name="SM_Pillar2" file="hangar_mesh/SM_Pillar2.obj"/>
+    <mesh name="SM_Pillar3" file="hangar_mesh/SM_Pillar3.obj"/>
+    <mesh name="SM_Pillar4" file="hangar_mesh/SM_Pillar4.obj"/>
+    <mesh name="SM_Pillar5" file="hangar_mesh/SM_Pillar5.obj"/>
+    <mesh name="SM_Pillar6" file="hangar_mesh/SM_Pillar6.obj"/>
+    <mesh name="SM_Pillar7" file="hangar_mesh/SM_Pillar7.obj"/>
+    <mesh name="SM_Pillar8" file="hangar_mesh/SM_Pillar8.obj"/>
+    <mesh name="SM_DoorSingle3" file="hangar_mesh/SM_DoorSingle3.obj"/>
+    <mesh name="SM_DoorDouble_26" file="hangar_mesh/SM_DoorDouble_26.obj"/>
+    <mesh name="SM_DoorDouble2" file="hangar_mesh/SM_DoorDouble2.obj"/>
+    <mesh name="SM_DoorSingle4" file="hangar_mesh/SM_DoorSingle4.obj"/>
+    <mesh name="SM_Stairs_32" file="hangar_mesh/SM_Stairs_32.obj"/>
+    <mesh name="SM_Stairs2" file="hangar_mesh/SM_Stairs2.obj"/>
+    <mesh name="SM_StairsRailing_Inner_2" file="hangar_mesh/SM_StairsRailing_Inner_2.obj"/>
+    <mesh name="SM_StairsRailing_Outer_7" file="hangar_mesh/SM_StairsRailing_Outer_7.obj"/>
+    <mesh name="SM_StairsRailing_Top_10" file="hangar_mesh/SM_StairsRailing_Top_10.obj"/>
+    <mesh name="SM_StairsRailing_Outer2" file="hangar_mesh/SM_StairsRailing_Outer2.obj"/>
+    <mesh name="SM_StairsRailing_Inner2" file="hangar_mesh/SM_StairsRailing_Inner2.obj"/>
+    <mesh name="SM_StairsRailing_Top2_15" file="hangar_mesh/SM_StairsRailing_Top2_15.obj"/>
+    <mesh name="SM_DoorSingle5" file="hangar_mesh/SM_DoorSingle5.obj"/>
+    <mesh name="SM_DoorSingle6" file="hangar_mesh/SM_DoorSingle6.obj"/>
+    <mesh name="SM_WallRailing_H_2" file="hangar_mesh/SM_WallRailing_H_2.obj"/>
+    <mesh name="SM_WallRailing_H2_5" file="hangar_mesh/SM_WallRailing_H2_5.obj"/>
+    <mesh name="SM_WallRailing_H3" file="hangar_mesh/SM_WallRailing_H3.obj"/>
+    <mesh name="SM_WallRailing_H4" file="hangar_mesh/SM_WallRailing_H4.obj"/>
+    <mesh name="SM_WallRailing_H5" file="hangar_mesh/SM_WallRailing_H5.obj"/>
+    <mesh name="SM_WallRailing_H6" file="hangar_mesh/SM_WallRailing_H6.obj"/>
+    <mesh name="SM_WallRailing_H7" file="hangar_mesh/SM_WallRailing_H7.obj"/>
+    <mesh name="SM_WallRailing_H8" file="hangar_mesh/SM_WallRailing_H8.obj"/>
+    <mesh name="SM_WallRailing_H9" file="hangar_mesh/SM_WallRailing_H9.obj"/>
+    <mesh name="SM_WallRailing_H10" file="hangar_mesh/SM_WallRailing_H10.obj"/>
+    <mesh name="SM_WallRailing_H11" file="hangar_mesh/SM_WallRailing_H11.obj"/>
+    <mesh name="SM_WallRailing_H12" file="hangar_mesh/SM_WallRailing_H12.obj"/>
+    <mesh name="SM_WallRailing_H13" file="hangar_mesh/SM_WallRailing_H13.obj"/>
+    <mesh name="SM_WallRailing_H14" file="hangar_mesh/SM_WallRailing_H14.obj"/>
+    <mesh name="SM_WallRailing_H15" file="hangar_mesh/SM_WallRailing_H15.obj"/>
+    <mesh name="SM_WallRailing_H16" file="hangar_mesh/SM_WallRailing_H16.obj"/>
+    <mesh name="SM_WallRailing_H17" file="hangar_mesh/SM_WallRailing_H17.obj"/>
+    <mesh name="SM_WallRailing_H18" file="hangar_mesh/SM_WallRailing_H18.obj"/>
+    <mesh name="SM_WallRailing_H19" file="hangar_mesh/SM_WallRailing_H19.obj"/>
+    <mesh name="SM_DoorDouble3" file="hangar_mesh/SM_DoorDouble3.obj"/>
+    <mesh name="SM_AccessStand2" file="hangar_mesh/SM_AccessStand2.obj"/>
+    <mesh name="SM_Ladder2" file="hangar_mesh/SM_Ladder2.obj"/>
+    <mesh name="SM_Ladder3" file="hangar_mesh/SM_Ladder3.obj"/>
+    <mesh name="SM_TrashBin2" file="hangar_mesh/SM_TrashBin2.obj"/>
+    <mesh name="SM_Box_C8" file="hangar_mesh/SM_Box_C8.obj"/>
+    <mesh name="SM_Box_C9" file="hangar_mesh/SM_Box_C9.obj"/>
+    <mesh name="SM_Box_C10" file="hangar_mesh/SM_Box_C10.obj"/>
+    <mesh name="SM_Box_B8" file="hangar_mesh/SM_Box_B8.obj"/>
+    <mesh name="SM_Railing_Corner6_20_GEN_VARIABLE_StaticMeshActor_CAT_486" file="hangar_mesh/SM_Railing_Corner6_20_GEN_VARIABLE_StaticMeshActor_CAT_486.obj"/>
+    <mesh name="SM_DockingStation_Stairs2_GEN_VARIABLE_StaticMeshActor_CAT_7726" file="hangar_mesh/SM_DockingStation_Stairs2_GEN_VARIABLE_StaticMeshActor_CAT_7726.obj"/>
+    <mesh name="SM_DockingStation_Top_50_GEN_VARIABLE_StaticMeshActor_CAT_7711" file="hangar_mesh/SM_DockingStation_Top_50_GEN_VARIABLE_StaticMeshActor_CAT_7711.obj"/>
+    <mesh name="SM_DockingStation_Stairs_46_GEN_VARIABLE_StaticMeshActo_37a21c2" file="hangar_mesh/SM_DockingStation_Stairs_46_GEN_VARIABLE_StaticMeshActo_37a21c2.obj"/>
+    <mesh name="SM_DockingStation_438_GEN_VARIABLE_StaticMeshActor_CAT_7694" file="hangar_mesh/SM_DockingStation_438_GEN_VARIABLE_StaticMeshActor_CAT_7694.obj"/>
+    <mesh name="SM_Cart2" file="hangar_mesh/SM_Cart2.obj"/>
+    <mesh name="SM_TrashBin4" file="hangar_mesh/SM_TrashBin4.obj"/>
+    <mesh name="SM_WallFront_382.003" file="hangar_mesh/SM_WallFront_382.003.obj"/>
+    <mesh name="SM_WallFront_382.00311" file="hangar_mesh/SM_WallFront_382.00311.obj"/>
+    <mesh name="SM_DoorSlide_399.001" file="hangar_mesh/SM_DoorSlide_399.001.obj"/>
+    <mesh name="SM_WallSide16.001" file="hangar_mesh/SM_WallSide16.001.obj"/>
+    <mesh name="SM_WallSide16.002" file="hangar_mesh/SM_WallSide16.002.obj"/>
+    <mesh name="collision_SM_WallSide4.001" file="hangar_mesh/collision_SM_WallSide4.001.obj"/>
+    <mesh name="SM_WallSide4.002" file="hangar_mesh/SM_WallSide4.002.obj"/>
+    <mesh name="SM_Lamps6.001" file="hangar_mesh/SM_Lamps6.001.obj"/>
+    <mesh name="SM_WallBack_396.001" file="hangar_mesh/SM_WallBack_396.001.obj"/>
+    <mesh name="SM_Roof16_360.001" file="hangar_mesh/SM_Roof16_360.001.obj"/>
+    <mesh name="SM_Roof4.001" file="hangar_mesh/SM_Roof4.001.obj"/>
+    <mesh name="Cube.003" file="hangar_mesh/Cube.003.obj"/>
+    <mesh name="collision_Plane" file="hangar_mesh/collision_Plane.obj"/>
+    <mesh name="collision_Plane.001" file="hangar_mesh/collision_Plane.001.obj"/>
+    <mesh name="collision_Plane.002" file="hangar_mesh/collision_Plane.002.obj"/>
+    <mesh name="collision_Plane.003" file="hangar_mesh/collision_Plane.003.obj"/>
+    <mesh name="collision_Plane.004" file="hangar_mesh/collision_Plane.004.obj"/>
+    <mesh name="collision_Plane.005" file="hangar_mesh/collision_Plane.005.obj"/>
+    <mesh name="collision_Plane.006" file="hangar_mesh/collision_Plane.006.obj"/>
+    <mesh name="collision_Plane.007" file="hangar_mesh/collision_Plane.007.obj"/>
+    <mesh name="collision_Plane.008" file="hangar_mesh/collision_Plane.008.obj"/>
+    <mesh name="collision_Plane.009" file="hangar_mesh/collision_Plane.009.obj"/>
+    <mesh name="collision_Cube" file="hangar_mesh/collision_Cube.obj"/>
+    <mesh name="collision_Cube.001" file="hangar_mesh/collision_Cube.001.obj"/>
+    <mesh name="collision_Cube.002" file="hangar_mesh/collision_Cube.002.obj"/>
+    <mesh name="collision_Cube.004" file="hangar_mesh/collision_Cube.004.obj"/>
+    <mesh name="collision_Cube.005" file="hangar_mesh/collision_Cube.005.obj"/>
+    <mesh name="collision_Cube.006" file="hangar_mesh/collision_Cube.006.obj"/>
+    <mesh name="collision_Cube.007" file="hangar_mesh/collision_Cube.007.obj"/>
+    <mesh name="collision_Cube.008" file="hangar_mesh/collision_Cube.008.obj"/>
+    <mesh name="collision_Cube.009" file="hangar_mesh/collision_Cube.009.obj"/>
+    <mesh name="collision_Cube.010" file="hangar_mesh/collision_Cube.010.obj"/>
+    <mesh name="collision_Cube.011" file="hangar_mesh/collision_Cube.011.obj"/>
+    <mesh name="collision_Cube.012" file="hangar_mesh/collision_Cube.012.obj"/>
+    <mesh name="collision_Cube.013" file="hangar_mesh/collision_Cube.013.obj"/>
+    <mesh name="collision_Cube.014" file="hangar_mesh/collision_Cube.014.obj"/>
+    <mesh name="collision_Cube.015" file="hangar_mesh/collision_Cube.015.obj"/>
+    <mesh name="collision_SM_Room_426.001" file="hangar_mesh/collision_SM_Room_426.001.obj"/>
+    <mesh name="SM_Room_426.002" file="hangar_mesh/SM_Room_426.002.obj"/>
+    <mesh name="collision_Plane.010" file="hangar_mesh/collision_Plane.010.obj"/>
+    <mesh name="collision_Plane.011" file="hangar_mesh/collision_Plane.011.obj"/>
+    <mesh name="collision_Plane.012" file="hangar_mesh/collision_Plane.012.obj"/>
+    <mesh name="collision_supports" file="hangar_mesh/collision_supports.obj"/>
+    <mesh name="collision_supports.001" file="hangar_mesh/collision_supports.001.obj"/>
+    <mesh name="collision_supports.002" file="hangar_mesh/collision_supports.002.obj"/>
+    <mesh name="collision_supports.003" file="hangar_mesh/collision_supports.003.obj"/>
+    <mesh name="collision_supports.004" file="hangar_mesh/collision_supports.004.obj"/>
+    <mesh name="collision_supports.005" file="hangar_mesh/collision_supports.005.obj"/>
+    <mesh name="collision_supports.006" file="hangar_mesh/collision_supports.006.obj"/>
+    <mesh name="collision_supports.007" file="hangar_mesh/collision_supports.007.obj"/>
+    <mesh name="collision_supports.008" file="hangar_mesh/collision_supports.008.obj"/>
+    <mesh name="collision_supports.009" file="hangar_mesh/collision_supports.009.obj"/>
+    <mesh name="collision_supports.010" file="hangar_mesh/collision_supports.010.obj"/>
+    <mesh name="collision_supports.011" file="hangar_mesh/collision_supports.011.obj"/>
+    <mesh name="collision_supports.012" file="hangar_mesh/collision_supports.012.obj"/>
+    <mesh name="collision_supports.013" file="hangar_mesh/collision_supports.013.obj"/>
+    <mesh name="collision_supports.014" file="hangar_mesh/collision_supports.014.obj"/>
+    <mesh name="collision_supports.015" file="hangar_mesh/collision_supports.015.obj"/>
+    <mesh name="SM_WallBack_396.002" file="hangar_mesh/SM_WallBack_396.002.obj"/>
+    <mesh name="SM_WallBack_396.003" file="hangar_mesh/SM_WallBack_396.003.obj"/>
+    <mesh name="SM_WallFront_382.002" file="hangar_mesh/SM_WallFront_382.002.obj"/>
+    <mesh name="collision_SM_WallFront_382.003" file="hangar_mesh/collision_SM_WallFront_382.003.obj"/>
+    <mesh name="collision_SM_Room_426.003" file="hangar_mesh/collision_SM_Room_426.003.obj"/>
+    <mesh name="collision_SM_Room_426.004" file="hangar_mesh/collision_SM_Room_426.004.obj"/>
+    <mesh name="collision_SM_Floor_376" file="hangar_mesh/collision_SM_Floor_376.obj"/>
+    <mesh name="SM_WallSide4.001" file="hangar_mesh/SM_WallSide4.001.obj"/>
+    <mesh name="collision_SM_WallBack_396.001" file="hangar_mesh/collision_SM_WallBack_396.001.obj"/>
+    <mesh name="collision_SM_WallBack_396.002" file="hangar_mesh/collision_SM_WallBack_396.002.obj"/>
+    <mesh name="collision_SM_WallSide16.002" file="hangar_mesh/collision_SM_WallSide16.002.obj"/>
+    <mesh name="collision_SM_WallFront_382.001" file="hangar_mesh/collision_SM_WallFront_382.001.obj"/>
+    <mesh name="collision_SM_WallFront_382.002" file="hangar_mesh/collision_SM_WallFront_382.002.obj"/>
+    <mesh name="SM_Box_A7_73.001" file="hangar_mesh/SM_Box_A7_73.001.obj"/>
+    <mesh name="SM_Box_A8.001" file="hangar_mesh/SM_Box_A8.001.obj"/>
+    <mesh name="SM_Box_C7_77.001" file="hangar_mesh/SM_Box_C7_77.001.obj"/>
+    <mesh name="collision_SM_Pillar4.001" file="hangar_mesh/collision_SM_Pillar4.001.obj"/>
+    <mesh name="collision_SM_Pillar3.001" file="hangar_mesh/collision_SM_Pillar3.001.obj"/>
+    <mesh name="collision_SM_Pillar2.001" file="hangar_mesh/collision_SM_Pillar2.001.obj"/>
+    <mesh name="collision_SM_Pillar_10.001" file="hangar_mesh/collision_SM_Pillar_10.001.obj"/>
+    <mesh name="collision_SM_Pillar8.001" file="hangar_mesh/collision_SM_Pillar8.001.obj"/>
+    <mesh name="collision_SM_Pillar7.001" file="hangar_mesh/collision_SM_Pillar7.001.obj"/>
+    <mesh name="collision_SM_Pillar6.001" file="hangar_mesh/collision_SM_Pillar6.001.obj"/>
+    <mesh name="collision_SM_Pillar5.001" file="hangar_mesh/collision_SM_Pillar5.001.obj"/>
+    <mesh name="tire_lndg_rear_l_in.109" file="hangar_mesh/tire_lndg_rear_l_in.109.obj"/>
+    <mesh name="tire_lndg_rear_l_in.001" file="hangar_mesh/tire_lndg_rear_l_in.001.obj"/>
+    <mesh name="tire_lndg_rear_l_in.002" file="hangar_mesh/tire_lndg_rear_l_in.002.obj"/>
+    <mesh name="tire_lndg_rear_l_in.003" file="hangar_mesh/tire_lndg_rear_l_in.003.obj"/>
+    <mesh name="tire_lndg_rear_l_in.004" file="hangar_mesh/tire_lndg_rear_l_in.004.obj"/>
+    <mesh name="tire_lndg_rear_l_in.005" file="hangar_mesh/tire_lndg_rear_l_in.005.obj"/>
+    <mesh name="tire_lndg_rear_l_in.006" file="hangar_mesh/tire_lndg_rear_l_in.006.obj"/>
+    <mesh name="tire_lndg_rear_l_in.007" file="hangar_mesh/tire_lndg_rear_l_in.007.obj"/>
+    <mesh name="tire_lndg_rear_l_in.008" file="hangar_mesh/tire_lndg_rear_l_in.008.obj"/>
+    <mesh name="tire_lndg_rear_l_in.009" file="hangar_mesh/tire_lndg_rear_l_in.009.obj"/>
+    <mesh name="tire_lndg_rear_l_in.010" file="hangar_mesh/tire_lndg_rear_l_in.010.obj"/>
+    <mesh name="tire_lndg_rear_l_in.011" file="hangar_mesh/tire_lndg_rear_l_in.011.obj"/>
+    <mesh name="tire_lndg_rear_l_in.012" file="hangar_mesh/tire_lndg_rear_l_in.012.obj"/>
+    <mesh name="tire_lndg_rear_l_in.013" file="hangar_mesh/tire_lndg_rear_l_in.013.obj"/>
+    <mesh name="collision_box_1" file="hangar_mesh/collision_box_1.obj"/>
+    <mesh name="collision_box_2" file="hangar_mesh/collision_box_2.obj"/>
+    <mesh name="collision_box_3" file="hangar_mesh/collision_box_3.obj"/>
+    <mesh name="collision_box_4" file="hangar_mesh/collision_box_4.obj"/>
+    <mesh name="collision_box_5" file="hangar_mesh/collision_box_5.obj"/>
+    <mesh name="collision_box_6" file="hangar_mesh/collision_box_6.obj"/>
+    <mesh name="collision_box_7" file="hangar_mesh/collision_box_7.obj"/>
+    <mesh name="collision_box_8" file="hangar_mesh/collision_box_8.obj"/>
+    <mesh name="box_1" file="hangar_mesh/box_1.obj"/>
+    <mesh name="box_2" file="hangar_mesh/box_2.obj"/>
+    <mesh name="box_3" file="hangar_mesh/box_3.obj"/>
+    <mesh name="box_4" file="hangar_mesh/box_4.obj"/>
+    <mesh name="box_5" file="hangar_mesh/box_5.obj"/>
+    <mesh name="box_6" file="hangar_mesh/box_6.obj"/>
+    <mesh name="box_7" file="hangar_mesh/box_7.obj"/>
+    <mesh name="box_8" file="hangar_mesh/box_8.obj"/>
+    <material name="metal_hangar" rgba="0.25 0.25 0.25 1.0" reflectance="0.8960324823856354" metallic="0.7795275449752808"/>
+    <material name="glass" rgba="0.8258873820304871 0.9459500908851624 0.9999716877937317 0.28333336114883423" reflectance="0.9740158449858427" metallic="0.0"/>
+    <texture type="2d" name="floor" file="assets/floor.png" mark="edge" width="2048" height="2048"/>
+    <material name="floor" texture="floor" texuniform="true" texrepeat="1 1" reflectance="0.4472135901451111" metallic="0.0"/>
+    <texture type="2d" name="walls" file="assets/walls.png" mark="edge" width="2048" height="2048"/>
+    <material name="walls" texture="walls" texuniform="true" texrepeat="1 1" reflectance="0.4472135901451111" metallic="0.0"/>
+    <material name="yellow metal" rgba="1.0 0.6478113532066345 0.015450038015842438 1.0" reflectance="0.880284458398819" metallic="0.8976377844810486"/>
+    <material name="light" rgba="0.9999716877937317 0.9999716877937317 0.9999716877937317 1.0" reflectance="0.5" metallic="0.0"/>
+    <material name="box" rgba="0.7859447598457336 0.521110475063324 0.2919864058494568 1.0" reflectance="0.4472135901451111" metallic="0.0"/>
+    <material name="yellow_tape" rgba="0.8001177906990051 0.6553049087524414 0.0 1.0" reflectance="0.18644070625305176" metallic="0.0"/>
+    <material name="Material.001" rgba="0.9999716877937317 0.9999716877937317 0.9999716877937317 1.0" reflectance="0.5" metallic="0.0"/>
+    <material name="rubber" rgba="0.01345729734748602 0.01345729734748602 0.01345729734748602 1.0" reflectance="0.5" metallic="0.0"/>
+    <material name="Material" rgba="0.1858992576599121 0.1858992576599121 0.1858992576599121 1.0" reflectance="1.0" metallic="0.06779661029577255"/>
   </asset>
 </mujoco>
diff --git a/src/hangar_sim/description/hangar_scene.xml b/src/hangar_sim/description/hangar_scene.xml
index a5c4d2e6..d2919648 100644
--- a/src/hangar_sim/description/hangar_scene.xml
+++ b/src/hangar_sim/description/hangar_scene.xml
@@ -1,98 +1,36 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco>
   <option>
-    <flag multiccd="enable" />
+    <flag multiccd="enable"/>
   </option>
-  <compiler angle="radian" meshdir="assets" autolimits="true" />
-  <include file="ur5e_ridgeback.xml" />
-  <include file="hangar.xml" />
-  <option integrator="implicit" timestep="0.01" />
+  <compiler angle="radian" meshdir="assets" autolimits="true"/>
+  <include file="ur5e_ridgeback.xml"/>
+  <include file="hangar.xml"/>
+  <option integrator="implicit" timestep="0.01"/>
   <visual>
-    <headlight diffuse="1.0 1.0 1.0" ambient="0.4 0.4 0.4" specular="0 0 0" />
-    <rgba haze="0.15 0.25 0.35 1" />
-    <global offwidth="1280" offheight="720" azimuth="0" elevation="180" />
-    <map znear=".0001" />
+    <headlight diffuse="1.0 1.0 1.0" ambient="0.4 0.4 0.4" specular="0 0 0"/>
+    <rgba haze="0.15 0.25 0.35 1"/>
+    <global offwidth="1280" offheight="720" azimuth="0" elevation="180"/>
+    <map znear=".0001"/>
   </visual>
 
   <worldbody>
     <!-- Scene Camera -->
-    <site
-      name="scene_camera_optical_frame"
-      pos="-14.4207 -4.39686 9.04094"
-      quat="0.415847 -0.883719 0.194298 -0.09143"
-    />
-    <camera
-      name="scene_camera"
-      pos="-14.4207 -4.39686 9.04094"
-      quat="0.883719 0.415847 -0.09143 -0.194298"
-      fovy="45"
-      mode="fixed"
-      resolution="1280 720"
-    />
+    <site name="scene_camera_optical_frame" pos="-14.4207 -4.39686 9.04094" quat="0.415847 -0.883719 0.194298 -0.09143"/>
+    <camera name="scene_camera" pos="-14.4207 -4.39686 9.04094" quat="0.883719 0.415847 -0.09143 -0.194298" fovy="45" mode="fixed" resolution="1280 720"/>
   </worldbody>
 
   <keyframe>
-    <key
-      name="default"
-      qpos="-0.0576239 -0.0172046 -0.0185516 0.00183643 -0.104441 -5.86852e-08 -5.78748e-08 -5.23725e-08 -4.36527e-08 -3.40517e-08 -2.61422e-08 -2.20435e-08 -2.28539e-08 -2.83562e-08 -0.574246 -0.581266 -5.45865e-08 -0.230591 3.23249e-08 2.59432e-08 1.70253e-08 7.96051e-09 1.17788e-09 -1.50522e-09 6.30151e-10 7.01182e-09 -0.230843 -0.260034 3.17772e-08 3.44603e-08 -0.188217 1.65809e-08 1.02713e-08 1.21264e-09 -8.16771e-09 -1.53564e-08 -1.84271e-08 -1.65571e-08 -1.02474e-08 -0.24846 -0.2671 1.53802e-08 1.84509e-08 -0.186578 -1.85093e-08 -1.66726e-08 -1.0385e-08 -1.33104e-09 8.06315e-09 1.52804e-08 1.83869e-08 1.65503e-08 1.02626e-08 -0.503391 -0.520458 -1.54028e-08 -0.00374438 -1.18339 1.00254 0.806741 1.5707 3.15041 -9.76535 3.7638 0.124902 0.97941 1.32151e-06 -9.14725e-06 0.201881 -9.42551 5.1824 0.124903 0.803499 9.50258e-06 -3.70911e-08 0.595306 -11.438 1.94748 0.78904 -0.383066 0.383808 0.594164 -0.594071 -12.0004 5.15872 0.124902 0.666052 -7.33131e-06 -6.66383e-06 0.745905 -12.0488 1.18718 0.125027 0.34658 -0.34669 -0.616068 0.616562 -11.1756 5.30231 0.124903 0.98111 -1.25581e-05 -2.16444e-06 -0.193453 -8.84404 3.16202 0.124904 0.964915 1.25055e-05 2.1434e-06 0.262563 -10.7473
-       0 -1.57 0 -0.707107 -0.707107 0"
-    />
+    <key name="default" qpos="-0.0576239 -0.0172046 -0.0185516 0.00183643 -0.104441 -5.86852e-08 -5.78748e-08 -5.23725e-08 -4.36527e-08 -3.40517e-08 -2.61422e-08 -2.20435e-08 -2.28539e-08 -2.83562e-08 -0.574246 -0.581266 -5.45865e-08 -0.230591 3.23249e-08 2.59432e-08 1.70253e-08 7.96051e-09 1.17788e-09 -1.50522e-09 6.30151e-10 7.01182e-09 -0.230843 -0.260034 3.17772e-08 3.44603e-08 -0.188217 1.65809e-08 1.02713e-08 1.21264e-09 -8.16771e-09 -1.53564e-08 -1.84271e-08 -1.65571e-08 -1.02474e-08 -0.24846 -0.2671 1.53802e-08 1.84509e-08 -0.186578 -1.85093e-08 -1.66726e-08 -1.0385e-08 -1.33104e-09 8.06315e-09 1.52804e-08 1.83869e-08 1.65503e-08 1.02626e-08 -0.503391 -0.520458 -1.54028e-08 -0.00374438 -1.18339 1.00254 0.806741 1.5707 3.15041 -9.76535 3.7638 0.124902 0.97941 1.32151e-06 -9.14725e-06 0.201881 -9.42551 5.1824 0.124903 0.803499 9.50258e-06 -3.70911e-08 0.595306 -11.438 1.94748 0.78904 -0.383066 0.383808 0.594164 -0.594071 -12.0004 5.15872 0.124902 0.666052 -7.33131e-06 -6.66383e-06 0.745905 -12.0488 1.18718 0.125027 0.34658 -0.34669 -0.616068 0.616562 -11.1756 5.30231 0.124903 0.98111 -1.25581e-05 -2.16444e-06 -0.193453 -8.84404 3.16202 0.124904 0.964915 1.25055e-05 2.1434e-06 0.262563 -10.7473        0 -1.57 0 -0.707107 -0.707107 0"/>
   </keyframe>
   <equality>
-    <weld
-      name="box_1_suction"
-      body1="collision_vacuum_suction_cups"
-      body2="collision_box_1"
-      active="false"
-      torquescale="20"
-    />
-    <weld
-      name="box_2_suction"
-      body1="collision_vacuum_suction_cups"
-      body2="collision_box_2"
-      active="false"
-      torquescale="20"
-    />
-    <weld
-      name="box_3_suction"
-      body1="collision_vacuum_suction_cups"
-      body2="collision_box_3"
-      active="false"
-      torquescale="20"
-    />
-    <weld
-      name="box_4_suction"
-      body1="collision_vacuum_suction_cups"
-      body2="collision_box_4"
-      active="false"
-      torquescale="20"
-    />
-    <weld
-      name="box_5_suction"
-      body1="collision_vacuum_suction_cups"
-      body2="collision_box_5"
-      active="false"
-      torquescale="20"
-    />
-    <weld
-      name="box_6_suction"
-      body1="collision_vacuum_suction_cups"
-      body2="collision_box_6"
-      active="false"
-      torquescale="20"
-    />
-    <weld
-      name="box_7_suction"
-      body1="collision_vacuum_suction_cups"
-      body2="collision_box_7"
-      active="false"
-      torquescale="20"
-    />
-    <weld
-      name="box_8_suction"
-      body1="collision_vacuum_suction_cups"
-      body2="collision_box_8"
-      active="false"
-      torquescale="20"
-    />
+    <weld name="box_1_suction" body1="collision_vacuum_suction_cups" body2="collision_box_1" active="false" torquescale="20"/>
+    <weld name="box_2_suction" body1="collision_vacuum_suction_cups" body2="collision_box_2" active="false" torquescale="20"/>
+    <weld name="box_3_suction" body1="collision_vacuum_suction_cups" body2="collision_box_3" active="false" torquescale="20"/>
+    <weld name="box_4_suction" body1="collision_vacuum_suction_cups" body2="collision_box_4" active="false" torquescale="20"/>
+    <weld name="box_5_suction" body1="collision_vacuum_suction_cups" body2="collision_box_5" active="false" torquescale="20"/>
+    <weld name="box_6_suction" body1="collision_vacuum_suction_cups" body2="collision_box_6" active="false" torquescale="20"/>
+    <weld name="box_7_suction" body1="collision_vacuum_suction_cups" body2="collision_box_7" active="false" torquescale="20"/>
+    <weld name="box_8_suction" body1="collision_vacuum_suction_cups" body2="collision_box_8" active="false" torquescale="20"/>
   </equality>
 </mujoco>
diff --git a/src/hangar_sim/description/rear_left_wheel_link.xml b/src/hangar_sim/description/rear_left_wheel_link.xml
index 2484daab..f62dbef2 100644
--- a/src/hangar_sim/description/rear_left_wheel_link.xml
+++ b/src/hangar_sim/description/rear_left_wheel_link.xml
@@ -1,357 +1,77 @@
+<?xml version='1.0' encoding='utf-8'?>
 <body name="rear_left_wheel_intermediate_link" pos="0 0 0">
-  <inertial
-    pos="0 0 0"
-    quat="0.707107 0 0 0.707107"
-    mass="1.0"
-    diaginertia="0.0524193 0.0303095 0.0303095"
-  />
-  <joint name="rear_left_wheel" pos="0 0 0" axis="0 1 0" />
+  <inertial pos="0 0 0" quat="0.707107 0 0 0.707107" mass="1.0" diaginertia="0.0524193 0.0303095 0.0303095"/>
+  <joint name="rear_left_wheel" pos="0 0 0" axis="0 1 0"/>
 
-  <body
-    name="rear_left_roller_0_link"
-    pos="0.04385159698893431 0.0 0.011749999999999998"
-  >
-    <joint
-      name="rear_left_slipping_0_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.006296806022131382 0.0435 0.023499999999999997"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_left_roller_0_link" pos="0.04385159698893431 0.0 0.011749999999999998">
+    <joint name="rear_left_slipping_0_joint" type="hinge" pos="0 0 0" axis="-0.006296806022131382 0.0435 0.023499999999999997" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_left_roller_1_link"
-    pos="0.03210159698893431 0.0 0.032101596988934306"
-  >
-    <joint
-      name="rear_left_slipping_1_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.01720319397786861 0.0435 0.017203193977868615"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_left_roller_1_link" pos="0.03210159698893431 0.0 0.032101596988934306">
+    <joint name="rear_left_slipping_1_joint" type="hinge" pos="0 0 0" axis="-0.01720319397786861 0.0435 0.017203193977868615" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_left_roller_2_link"
-    pos="0.011750000000000005 0.0 0.0438515969889343"
-  >
-    <joint
-      name="rear_left_slipping_2_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.023500000000000004 0.0435 0.006296806022131389"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_left_roller_2_link" pos="0.011750000000000005 0.0 0.0438515969889343">
+    <joint name="rear_left_slipping_2_joint" type="hinge" pos="0 0 0" axis="-0.023500000000000004 0.0435 0.006296806022131389" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_left_roller_3_link"
-    pos="-0.011749999999999993 0.0 0.04385159698893431"
-  >
-    <joint
-      name="rear_left_slipping_3_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.023499999999999993 0.0435 -0.006296806022131382"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_left_roller_3_link" pos="-0.011749999999999993 0.0 0.04385159698893431">
+    <joint name="rear_left_slipping_3_joint" type="hinge" pos="0 0 0" axis="-0.023499999999999993 0.0435 -0.006296806022131382" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_left_roller_4_link"
-    pos="-0.0321015969889343 0.0 0.03210159698893432"
-  >
-    <joint
-      name="rear_left_slipping_4_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.01720319397786862 0.0435 -0.0172031939778686"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_left_roller_4_link" pos="-0.0321015969889343 0.0 0.03210159698893432">
+    <joint name="rear_left_slipping_4_joint" type="hinge" pos="0 0 0" axis="-0.01720319397786862 0.0435 -0.0172031939778686" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_left_roller_5_link"
-    pos="-0.0438515969889343 0.0 0.011750000000000012"
-  >
-    <joint
-      name="rear_left_slipping_5_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.006296806022131389 0.0435 -0.02350000000000001"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_left_roller_5_link" pos="-0.0438515969889343 0.0 0.011750000000000012">
+    <joint name="rear_left_slipping_5_joint" type="hinge" pos="0 0 0" axis="-0.006296806022131389 0.0435 -0.02350000000000001" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_left_roller_6_link"
-    pos="-0.043851596988934316 0.0 -0.011749999999999991"
-  >
-    <joint
-      name="rear_left_slipping_6_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.006296806022131375 0.0435 -0.023499999999999993"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_left_roller_6_link" pos="-0.043851596988934316 0.0 -0.011749999999999991">
+    <joint name="rear_left_slipping_6_joint" type="hinge" pos="0 0 0" axis="0.006296806022131375 0.0435 -0.023499999999999993" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_left_roller_7_link"
-    pos="-0.03210159698893432 0.0 -0.03210159698893429"
-  >
-    <joint
-      name="rear_left_slipping_7_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.017203193977868604 0.0435 -0.017203193977868618"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_left_roller_7_link" pos="-0.03210159698893432 0.0 -0.03210159698893429">
+    <joint name="rear_left_slipping_7_joint" type="hinge" pos="0 0 0" axis="0.017203193977868604 0.0435 -0.017203193977868618" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_left_roller_8_link"
-    pos="-0.011750000000000014 0.0 -0.0438515969889343"
-  >
-    <joint
-      name="rear_left_slipping_8_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.023500000000000014 0.0435 -0.006296806022131396"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_left_roller_8_link" pos="-0.011750000000000014 0.0 -0.0438515969889343">
+    <joint name="rear_left_slipping_8_joint" type="hinge" pos="0 0 0" axis="0.023500000000000014 0.0435 -0.006296806022131396" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_left_roller_9_link"
-    pos="0.01174999999999998 0.0 -0.043851596988934316"
-  >
-    <joint
-      name="rear_left_slipping_9_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.023499999999999976 0.0435 0.006296806022131368"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_left_roller_9_link" pos="0.01174999999999998 0.0 -0.043851596988934316">
+    <joint name="rear_left_slipping_9_joint" type="hinge" pos="0 0 0" axis="0.023499999999999976 0.0435 0.006296806022131368" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_left_roller_10_link"
-    pos="0.032101596988934285 0.0 -0.032101596988934326"
-  >
-    <joint
-      name="rear_left_slipping_10_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.017203193977868635 0.0435 0.01720319397786861"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_left_roller_10_link" pos="0.032101596988934285 0.0 -0.032101596988934326">
+    <joint name="rear_left_slipping_10_joint" type="hinge" pos="0 0 0" axis="0.017203193977868635 0.0435 0.01720319397786861" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_left_roller_11_link"
-    pos="0.0438515969889343 0.0 -0.01175000000000001"
-  >
-    <joint
-      name="rear_left_slipping_11_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.006296806022131396 0.0435 0.02350000000000002"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_left_roller_11_link" pos="0.0438515969889343 0.0 -0.01175000000000001">
+    <joint name="rear_left_slipping_11_joint" type="hinge" pos="0 0 0" axis="0.006296806022131396 0.0435 0.02350000000000002" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 </body>
diff --git a/src/hangar_sim/description/rear_right_wheel_link.xml b/src/hangar_sim/description/rear_right_wheel_link.xml
index c044b53c..d0038635 100644
--- a/src/hangar_sim/description/rear_right_wheel_link.xml
+++ b/src/hangar_sim/description/rear_right_wheel_link.xml
@@ -1,357 +1,77 @@
+<?xml version='1.0' encoding='utf-8'?>
 <body name="rear_right_wheel_intermediate_link" pos="0 0 0">
-  <inertial
-    pos="0 0 0"
-    quat="0.707107 0 0 0.707107"
-    mass="1.0"
-    diaginertia="0.0524193 0.0303095 0.0303095"
-  />
-  <joint name="rear_right_wheel" pos="0 0 0" axis="0 1 0" />
+  <inertial pos="0 0 0" quat="0.707107 0 0 0.707107" mass="1.0" diaginertia="0.0524193 0.0303095 0.0303095"/>
+  <joint name="rear_right_wheel" pos="0 0 0" axis="0 1 0"/>
 
-  <body
-    name="rear_right_roller_0_link"
-    pos="0.0438515969889343 0.0 -0.01175000000000001"
-  >
-    <joint
-      name="rear_right_slipping_0_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.006296806022131396 0.0435 -0.02350000000000002"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_right_roller_0_link" pos="0.0438515969889343 0.0 -0.01175000000000001">
+    <joint name="rear_right_slipping_0_joint" type="hinge" pos="0 0 0" axis="-0.006296806022131396 0.0435 -0.02350000000000002" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_right_roller_1_link"
-    pos="0.04385159698893431 0.0 0.011749999999999998"
-  >
-    <joint
-      name="rear_right_slipping_1_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.006296806022131382 0.0435 -0.023499999999999997"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_right_roller_1_link" pos="0.04385159698893431 0.0 0.011749999999999998">
+    <joint name="rear_right_slipping_1_joint" type="hinge" pos="0 0 0" axis="0.006296806022131382 0.0435 -0.023499999999999997" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_right_roller_2_link"
-    pos="0.03210159698893431 0.0 0.032101596988934306"
-  >
-    <joint
-      name="rear_right_slipping_2_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.01720319397786861 0.0435 -0.017203193977868615"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_right_roller_2_link" pos="0.03210159698893431 0.0 0.032101596988934306">
+    <joint name="rear_right_slipping_2_joint" type="hinge" pos="0 0 0" axis="0.01720319397786861 0.0435 -0.017203193977868615" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_right_roller_3_link"
-    pos="0.011750000000000005 0.0 0.0438515969889343"
-  >
-    <joint
-      name="rear_right_slipping_3_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.023500000000000004 0.0435 -0.006296806022131389"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_right_roller_3_link" pos="0.011750000000000005 0.0 0.0438515969889343">
+    <joint name="rear_right_slipping_3_joint" type="hinge" pos="0 0 0" axis="0.023500000000000004 0.0435 -0.006296806022131389" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_right_roller_4_link"
-    pos="-0.011749999999999993 0.0 0.04385159698893431"
-  >
-    <joint
-      name="rear_right_slipping_4_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.023499999999999993 0.0435 0.006296806022131382"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_right_roller_4_link" pos="-0.011749999999999993 0.0 0.04385159698893431">
+    <joint name="rear_right_slipping_4_joint" type="hinge" pos="0 0 0" axis="0.023499999999999993 0.0435 0.006296806022131382" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_right_roller_5_link"
-    pos="-0.0321015969889343 0.0 0.03210159698893432"
-  >
-    <joint
-      name="rear_right_slipping_5_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.01720319397786862 0.0435 0.0172031939778686"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_right_roller_5_link" pos="-0.0321015969889343 0.0 0.03210159698893432">
+    <joint name="rear_right_slipping_5_joint" type="hinge" pos="0 0 0" axis="0.01720319397786862 0.0435 0.0172031939778686" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_right_roller_6_link"
-    pos="-0.0438515969889343 0.0 0.01175000000000001"
-  >
-    <joint
-      name="rear_right_slipping_6_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.006296806022131389 0.0435 0.02350000000000001"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_right_roller_6_link" pos="-0.0438515969889343 0.0 0.01175000000000001">
+    <joint name="rear_right_slipping_6_joint" type="hinge" pos="0 0 0" axis="0.006296806022131389 0.0435 0.02350000000000001" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_right_roller_7_link"
-    pos="-0.043851596988934316 0.0 -0.01174999999999999"
-  >
-    <joint
-      name="rear_right_slipping_7_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.006296806022131375 0.0435 0.023499999999999993"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_right_roller_7_link" pos="-0.043851596988934316 0.0 -0.01174999999999999">
+    <joint name="rear_right_slipping_7_joint" type="hinge" pos="0 0 0" axis="-0.006296806022131375 0.0435 0.023499999999999993" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_right_roller_8_link"
-    pos="-0.03210159698893432 0.0 -0.03210159698893429"
-  >
-    <joint
-      name="rear_right_slipping_8_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.017203193977868604 0.0435 0.017203193977868618"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_right_roller_8_link" pos="-0.03210159698893432 0.0 -0.03210159698893429">
+    <joint name="rear_right_slipping_8_joint" type="hinge" pos="0 0 0" axis="-0.017203193977868604 0.0435 0.017203193977868618" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_right_roller_9_link"
-    pos="-0.011750000000000016 0.0 -0.0438515969889343"
-  >
-    <joint
-      name="rear_right_slipping_9_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.023500000000000014 0.0435 0.006296806022131396"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_right_roller_9_link" pos="-0.011750000000000016 0.0 -0.0438515969889343">
+    <joint name="rear_right_slipping_9_joint" type="hinge" pos="0 0 0" axis="-0.023500000000000014 0.0435 0.006296806022131396" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_right_roller_10_link"
-    pos="0.011749999999999981 0.0 -0.043851596988934316"
-  >
-    <joint
-      name="rear_right_slipping_10_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.023499999999999976 0.0435 -0.006296806022131368"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_right_roller_10_link" pos="0.011749999999999981 0.0 -0.043851596988934316">
+    <joint name="rear_right_slipping_10_joint" type="hinge" pos="0 0 0" axis="-0.023499999999999976 0.0435 -0.006296806022131368" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 
-  <body
-    name="rear_right_roller_11_link"
-    pos="0.032101596988934285 0.0 -0.032101596988934326"
-  >
-    <joint
-      name="rear_right_slipping_11_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.017203193977868635 0.0435 -0.01720319397786861"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
+  <body name="rear_right_roller_11_link" pos="0.032101596988934285 0.0 -0.032101596988934326">
+    <joint name="rear_right_slipping_11_joint" type="hinge" pos="0 0 0" axis="-0.017203193977868635 0.0435 -0.01720319397786861" damping="0.1" limited="false" actuatorfrclimited="false"/>
+    <inertial pos="0 0 0" quat="0.711549 0.711549 0 0 " mass="0.001" diaginertia="0.00001 0.00001 0.00001"/>
+    <geom size="0.08" quat="1 0 0 0" type="sphere" rgba="0.2 0.2 0.2 1" contype="1" conaffinity="0"/>
   </body>
 </body>
diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml
index d370ff31..43abb438 100644
--- a/src/hangar_sim/description/ur5e_ridgeback.xml
+++ b/src/hangar_sim/description/ur5e_ridgeback.xml
@@ -1,201 +1,128 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="ur5e_ridgeback">
-  <compiler angle="radian" meshdir="assets" autolimits="true" />
+  <compiler angle="radian" meshdir="assets" autolimits="true"/>
 
-  <option integrator="implicitfast" />
+  <option integrator="implicitfast"/>
 
   <default>
     <default class="ur5e">
-      <material specular="0.5" shininess="0.25" />
-      <joint axis="0 1 0" range="-6.28319 6.28319" armature="0.1" />
-      <general
-        gaintype="fixed"
-        biastype="affine"
-        ctrlrange="-6.2831 6.2831"
-        gainprm="2000"
-        biasprm="0 -2000 -400"
-        forcerange="-50000 50000"
-      />
+      <material specular="0.5" shininess="0.25"/>
+      <joint axis="0 1 0" range="-6.28319 6.28319" armature="0.1"/>
+      <general gaintype="fixed" biastype="affine" ctrlrange="-6.2831 6.2831" gainprm="2000" biasprm="0 -2000 -400" forcerange="-50000 50000"/>
       <default class="size3">
         <default class="size3_limited">
-          <joint range="-3.1415 3.1415" />
-          <general ctrlrange="-3.1415 3.1415" />
+          <joint range="-3.1415 3.1415"/>
+          <general ctrlrange="-3.1415 3.1415"/>
         </default>
       </default>
       <default class="visual">
-        <geom type="mesh" contype="0" conaffinity="0" group="2" />
+        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
       </default>
       <default class="collision">
-        <geom type="capsule" group="3" />
+        <geom type="capsule" group="3"/>
         <default class="eef_collision">
-          <geom type="cylinder" />
+          <geom type="cylinder"/>
         </default>
       </default>
-      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4" />
+      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
     </default>
 
     <!-- robotiq 2f85 defaults -->
     <default class="2f85">
-      <mesh scale="0.001 0.001 0.001" />
-      <general biastype="affine" />
+      <mesh scale="0.001 0.001 0.001"/>
+      <general biastype="affine"/>
 
-      <joint axis="1 0 0" />
+      <joint axis="1 0 0"/>
       <default class="knuckle_link">
-        <joint
-          range="0 0.8"
-          armature="0.005"
-          damping="0.1"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="0 0.8" armature="0.005" damping="0.1" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
       <default class="finger_tip_link">
-        <joint
-          range="-0.872664 0.872664"
-          armature="0.001"
-          pos="0 -0.018 0.0065"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="-0.872664 0.872664" armature="0.001" pos="0 -0.018 0.0065" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
       <default class="inner_knuckle_link">
-        <joint
-          range="-0.29670597283 0.8"
-          armature="0.001"
-          stiffness="0.05"
-          springref="2.62"
-          damping="0.00125"
-        />
+        <joint range="-0.29670597283 0.8" armature="0.001" stiffness="0.05" springref="2.62" damping="0.00125"/>
       </default>
       <default class="finger_link">
-        <joint
-          range="-1.57 0"
-          armature="0.001"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="-1.57 0" armature="0.001" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
 
       <default class="2f85_visual">
-        <geom type="mesh" contype="0" conaffinity="0" group="2" />
+        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
       </default>
       <default class="2f85_collision">
-        <geom type="mesh" group="3" condim="6" />
+        <geom type="mesh" group="3" condim="6"/>
         <default class="pad_box1">
-          <geom
-            mass="0"
-            type="box"
-            pos="0 -0.0026 0.028125"
-            size="0.011 0.004 0.009375"
-            friction="0.7"
-            solimp="0.95 0.99 0.001"
-            solref="0.004 1"
-            priority="1"
-            rgba="0.55 0.55 0.55 1"
-          />
+          <geom mass="0" type="box" pos="0 -0.0026 0.028125" size="0.011 0.004 0.009375" friction="0.7" solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.55 0.55 0.55 1"/>
         </default>
         <default class="pad_box2">
-          <geom
-            mass="0"
-            type="box"
-            pos="0 -0.0026 0.009375"
-            size="0.011 0.004 0.009375"
-            friction="0.6"
-            solimp="0.95 0.99 0.001"
-            solref="0.004 1"
-            priority="1"
-            rgba="0.45 0.45 0.45 1"
-          />
+          <geom mass="0" type="box" pos="0 -0.0026 0.009375" size="0.011 0.004 0.009375" friction="0.6" solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.45 0.45 0.45 1"/>
         </default>
       </default>
     </default>
     <default class="robot">
-      <material specular="0.5" shininess="0.25" />
-      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973" />
-      <general
-        dyntype="none"
-        biastype="affine"
-        ctrlrange="-3.14159 3.14159"
-        forcerange="-50000 50000"
-      />
+      <material specular="0.5" shininess="0.25"/>
+      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
+      <general dyntype="none" biastype="affine" ctrlrange="-3.14159 3.14159" forcerange="-50000 50000"/>
       <default class="limitedJoints">
-        <joint limited="true" />
+        <joint limited="true"/>
       </default>
-      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4" />
+      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
     </default>
     <default class="rail">
-      <joint armature="0.1" damping="1" axis="0 0 1" range="-5.0 5.0" />
-      <general
-        dyntype="none"
-        biastype="affine"
-        ctrlrange="-5.0 5.0"
-        forcerange="-50000 50000"
-      />
+      <joint armature="0.1" damping="1" axis="0 0 1" range="-5.0 5.0"/>
+      <general dyntype="none" biastype="affine" ctrlrange="-5.0 5.0" forcerange="-50000 50000"/>
     </default>
   </default>
   <asset>
-    <material name="metal" rgba="0.58 0.58 0.58 1" />
-    <material name="silicone" rgba="0.1882 0.1882 0.1882 1" />
-    <material name="white" rgba="1.0 1.0 1.0 1" />
-    <material name="gray" rgba="0.4627 0.4627 0.4627 1" />
-    <material class="ur5e" name="black" rgba="0.033 0.033 0.033 1" />
-    <material class="ur5e" name="jointgray" rgba="0.278 0.278 0.278 1" />
-    <material class="ur5e" name="linkgray" rgba="0.82 0.82 0.82 1" />
-    <material class="ur5e" name="urblue" rgba="0.49 0.678 0.8 1" />
-    <material
-      name="dark metal"
-      rgba="0.04245108366012573 0.04245108366012573 0.04245108366012573 1.0"
-      reflectance="0.5"
-      metallic="0.0"
-    />
-    <material
-      name="plastic"
-      rgba="0.9433449506759644 0.9433449506759644 0.9433449506759644 1.0"
-      reflectance="0.5"
-      metallic="0.0"
-    />
-    <mesh name="base" file="base.stl" scale="0.001 0.001 0.001" />
-    <mesh name="base_platform" file="base_platform.stl" />
-    <mesh file="base_0.obj" />
-    <mesh file="base_1.obj" />
-    <mesh file="shoulder_0.obj" />
-    <mesh file="shoulder_1.obj" />
-    <mesh file="shoulder_2.obj" />
-    <mesh file="upperarm_0.obj" />
-    <mesh file="upperarm_1.obj" />
-    <mesh file="upperarm_2.obj" />
-    <mesh file="upperarm_3.obj" />
-    <mesh file="forearm_0.obj" />
-    <mesh file="forearm_1.obj" />
-    <mesh file="forearm_2.obj" />
-    <mesh file="forearm_3.obj" />
-    <mesh file="wrist1_0.obj" />
-    <mesh file="wrist1_1.obj" />
-    <mesh file="wrist1_2.obj" />
-    <mesh file="wrist2_0.obj" />
-    <mesh file="wrist2_1.obj" />
-    <mesh file="wrist2_2.obj" />
-    <mesh file="wrist3.obj" />
+    <material name="metal" rgba="0.58 0.58 0.58 1"/>
+    <material name="silicone" rgba="0.1882 0.1882 0.1882 1"/>
+    <material name="white" rgba="1.0 1.0 1.0 1"/>
+    <material name="gray" rgba="0.4627 0.4627 0.4627 1"/>
+    <material class="ur5e" name="black" rgba="0.033 0.033 0.033 1"/>
+    <material class="ur5e" name="jointgray" rgba="0.278 0.278 0.278 1"/>
+    <material class="ur5e" name="linkgray" rgba="0.82 0.82 0.82 1"/>
+    <material class="ur5e" name="urblue" rgba="0.49 0.678 0.8 1"/>
+    <material name="dark metal" rgba="0.04245108366012573 0.04245108366012573 0.04245108366012573 1.0" reflectance="0.5" metallic="0.0"/>
+    <material name="plastic" rgba="0.9433449506759644 0.9433449506759644 0.9433449506759644 1.0" reflectance="0.5" metallic="0.0"/>
+    <mesh name="base" file="base.stl" scale="0.001 0.001 0.001"/>
+    <mesh name="base_platform" file="base_platform.stl"/>
+    <mesh file="base_0.obj"/>
+    <mesh file="base_1.obj"/>
+    <mesh file="shoulder_0.obj"/>
+    <mesh file="shoulder_1.obj"/>
+    <mesh file="shoulder_2.obj"/>
+    <mesh file="upperarm_0.obj"/>
+    <mesh file="upperarm_1.obj"/>
+    <mesh file="upperarm_2.obj"/>
+    <mesh file="upperarm_3.obj"/>
+    <mesh file="forearm_0.obj"/>
+    <mesh file="forearm_1.obj"/>
+    <mesh file="forearm_2.obj"/>
+    <mesh file="forearm_3.obj"/>
+    <mesh file="wrist1_0.obj"/>
+    <mesh file="wrist1_1.obj"/>
+    <mesh file="wrist1_2.obj"/>
+    <mesh file="wrist2_0.obj"/>
+    <mesh file="wrist2_1.obj"/>
+    <mesh file="wrist2_2.obj"/>
+    <mesh file="wrist3.obj"/>
     <!-- Ridgeback -->
-    <mesh name="chassis_link" file="ridgeback/body.stl" />
-    <mesh name="top_link" file="ridgeback/top.stl" />
-    <mesh name="summit_xls_omni_wheel_1" file="omni_wheel_1.stl" />
-    <mesh name="summit_xls_omni_wheel_2" file="omni_wheel_2.stl" />
-    <mesh name="ham_assem" file="HAM_Assem.stl" />
-    <mesh name="camera_adapter" file="camera_adapter.stl" />
-    <mesh name="vacuum_base" file="vacuum_base.obj" />
-    <mesh name="vacuum_base_top" file="vacuum_base_top.obj" />
-    <mesh name="vacuum_suction_cups" file="vacuum_suction_cups.obj" />
-    <mesh name="collision_vacuum_base" file="collision_vacuum_base.obj" />
-    <mesh
-      name="collision_vacuum_suction_cups"
-      file="collision_vacuum_suction_cups.obj"
-      scale="1.0 1.0 1.0"
-    />
+    <mesh name="chassis_link" file="ridgeback/body.stl"/>
+    <mesh name="top_link" file="ridgeback/top.stl"/>
+    <mesh name="summit_xls_omni_wheel_1" file="omni_wheel_1.stl"/>
+    <mesh name="summit_xls_omni_wheel_2" file="omni_wheel_2.stl"/>
+    <mesh name="ham_assem" file="HAM_Assem.stl"/>
+    <mesh name="camera_adapter" file="camera_adapter.stl"/>
+    <mesh name="vacuum_base" file="vacuum_base.obj"/>
+    <mesh name="vacuum_base_top" file="vacuum_base_top.obj"/>
+    <mesh name="vacuum_suction_cups" file="vacuum_suction_cups.obj"/>
+    <mesh name="collision_vacuum_base" file="collision_vacuum_base.obj"/>
+    <mesh name="collision_vacuum_suction_cups" file="collision_vacuum_suction_cups.obj" scale="1.0 1.0 1.0"/>
   </asset>
 
   <worldbody>
     <body name="base_platform_rotation" pos="0 0 0" euler="0 0 0">
-      <inertial mass="0.01" pos="0 0 0" diaginertia="0.001 0.001 0.001" />
+      <inertial mass="0.01" pos="0 0 0" diaginertia="0.001 0.001 0.001"/>
       <!--      <joint-->
       <!--        type="free"-->
       <!--        stiffness="0"-->
@@ -203,289 +130,98 @@
       <!--        frictionloss="0"-->
       <!--        armature="0"-->
       <!--      />-->
-      <joint name="linear_x_joint" type="slide" axis="1 0 0" />
-      <joint name="linear_y_joint" type="slide" axis="0 1 0" />
-      <joint name="rotational_yaw_joint" type="hinge" axis="0 0 1" />
+      <joint name="linear_x_joint" type="slide" axis="1 0 0"/>
+      <joint name="linear_y_joint" type="slide" axis="0 1 0"/>
+      <joint name="rotational_yaw_joint" type="hinge" axis="0 0 1"/>
       <body name="base_platform" pos="0 0 0" euler="0 0 0">
         <body name="ridgeback_base_link">
-          <geom
-            name="chassis_link"
-            mesh="chassis_link"
-            material="black"
-            type="mesh"
-            contype="0"
-            conaffinity="0"
-            group="2"
-          />
-          <geom
-            name="riser_link"
-            size="0.2465 0.2465 0.0275"
-            pos="0 0 0.2475"
-            quat="0.92388 0 0 0.382683"
-            type="box"
-            rgba="0.4 0.4 0.4 1"
-          />
-          <geom
-            name="top_link"
-            mesh="top_link"
-            material="black"
-            type="mesh"
-            contype="0"
-            conaffinity="0"
-            group="2"
-          />
+          <geom name="chassis_link" mesh="chassis_link" material="black" type="mesh" contype="0" conaffinity="0" group="2"/>
+          <geom name="riser_link" size="0.2465 0.2465 0.0275" pos="0 0 0.2475" quat="0.92388 0 0 0.382683" type="box" rgba="0.4 0.4 0.4 1"/>
+          <geom name="top_link" mesh="top_link" material="black" type="mesh" contype="0" conaffinity="0" group="2"/>
           <body name="front_rocker_link" pos="0.319 0 0.05">
-            <inertial
-              pos="0 0 0"
-              quat="0.475329 0.523511 0.47531 0.523526"
-              mass="10.267"
-              diaginertia="0.432583 0.412817 0.0288"
-            />
-            <joint
-              name="front_rocker"
-              pos="0 0 0"
-              axis="1 0 0"
-              range="-0.08726 0.08726"
-            />
+            <inertial pos="0 0 0" quat="0.475329 0.523511 0.47531 0.523526" mass="10.267" diaginertia="0.432583 0.412817 0.0288"/>
+            <joint name="front_rocker" pos="0 0 0" axis="1 0 0" range="-0.08726 0.08726"/>
             <body name="front_left_wheel_link" pos="0 0.2755 0.07">
-              <include file="front_left_wheel_link.xml" />
+              <include file="front_left_wheel_link.xml"/>
             </body>
             <body name="front_right_wheel_link" pos="0 -0.2755 0.07">
-              <include file="front_right_wheel_link.xml" />
+              <include file="front_right_wheel_link.xml"/>
             </body>
           </body>
           <body name="rear_left_wheel_link" pos="-0.319 0.2755 0.12">
-            <include file="rear_left_wheel_link.xml" />
+            <include file="rear_left_wheel_link.xml"/>
           </body>
           <body name="rear_right_wheel_link" pos="-0.319 -0.2755 0.12">
-            <include file="rear_right_wheel_link.xml" />
+            <include file="rear_right_wheel_link.xml"/>
           </body>
           <body name="ham_assem" pos="0 0 0.278813" euler="1.5708 -1.57079 0">
-            <geom
-              name="ham_assem"
-              mesh="ham_assem"
-              type="mesh"
-              contype="0"
-              conaffinity="0"
-              group="2"
-            />
-            <geom
-              name="ham_assem_collision_1"
-              mass="0"
-              type="box"
-              pos="0 0.60358  0.148988"
-              size="0.395 0.01 0.307422"
-              group="3"
-            />
-            <geom
-              name="ham_assem_collision_2"
-              mass="0"
-              type="box"
-              pos="-0.26118 0.60358 -0.299002"
-              size="0.133638 0.01 0.140708"
-              group="3"
-            />
-            <geom
-              name="ham_assem_collision_3"
-              mass="0"
-              type="box"
-              pos="0.26118 0.60358 -0.299002"
-              size="0.133638 0.01 0.140708"
-              group="3"
-            />
+            <geom name="ham_assem" mesh="ham_assem" type="mesh" contype="0" conaffinity="0" group="2"/>
+            <geom name="ham_assem_collision_1" mass="0" type="box" pos="0 0.60358  0.148988" size="0.395 0.01 0.307422" group="3"/>
+            <geom name="ham_assem_collision_2" mass="0" type="box" pos="-0.26118 0.60358 -0.299002" size="0.133638 0.01 0.140708" group="3"/>
+            <geom name="ham_assem_collision_3" mass="0" type="box" pos="0.26118 0.60358 -0.299002" size="0.133638 0.01 0.140708" group="3"/>
 
             <!-- UR5E -->
-            <body
-              name="base"
-              pos="0 0.51905 -0.278813"
-              euler="-1.5708 0 1.5708"
-              childclass="ur5e"
-            >
-              <inertial
-                mass="4.0"
-                pos="0 0 0"
-                diaginertia="0.00443333156 0.00443333156 0.0072"
-              />
-              <geom mesh="base_0" material="black" class="visual" />
-              <geom mesh="base_1" material="jointgray" class="visual" />
+            <body name="base" pos="0 0.51905 -0.278813" euler="-1.5708 0 1.5708" childclass="ur5e">
+              <inertial mass="4.0" pos="0 0 0" diaginertia="0.00443333156 0.00443333156 0.0072"/>
+              <geom mesh="base_0" material="black" class="visual"/>
+              <geom mesh="base_1" material="jointgray" class="visual"/>
               <body name="shoulder_link" gravcomp="1" pos="0 0 0.163">
-                <inertial
-                  mass="3.7"
-                  pos="0 0 0"
-                  diaginertia="0.0102675 0.0102675 0.00666"
-                />
-                <joint name="shoulder_pan_joint" class="size3" axis="0 0 1" />
-                <geom mesh="shoulder_0" material="urblue" class="visual" />
-                <geom mesh="shoulder_1" material="black" class="visual" />
-                <geom mesh="shoulder_2" material="jointgray" class="visual" />
-                <geom class="collision" size="0.06 0.06" pos="0 0 -0.04" />
-                <body
-                  name="upper_arm_link"
-                  gravcomp="1"
-                  pos="0 0.138 0"
-                  quat="1 0 1 0"
-                >
-                  <inertial
-                    mass="8.393"
-                    pos="0 0 0.2125"
-                    diaginertia="0.133886 0.133886 0.0151074"
-                  />
-                  <joint name="shoulder_lift_joint" class="size3" />
-                  <geom mesh="upperarm_0" material="linkgray" class="visual" />
-                  <geom mesh="upperarm_1" material="black" class="visual" />
-                  <geom mesh="upperarm_2" material="jointgray" class="visual" />
-                  <geom mesh="upperarm_3" material="urblue" class="visual" />
-                  <geom
-                    class="collision"
-                    pos="0 -0.04 0"
-                    quat="1 1 0 0"
-                    size="0.06 0.06"
-                  />
-                  <geom class="collision" size="0.05 0.2" pos="0 0 0.2" />
+                <inertial mass="3.7" pos="0 0 0" diaginertia="0.0102675 0.0102675 0.00666"/>
+                <joint name="shoulder_pan_joint" class="size3" axis="0 0 1"/>
+                <geom mesh="shoulder_0" material="urblue" class="visual"/>
+                <geom mesh="shoulder_1" material="black" class="visual"/>
+                <geom mesh="shoulder_2" material="jointgray" class="visual"/>
+                <geom class="collision" size="0.06 0.06" pos="0 0 -0.04"/>
+                <body name="upper_arm_link" gravcomp="1" pos="0 0.138 0" quat="1 0 1 0">
+                  <inertial mass="8.393" pos="0 0 0.2125" diaginertia="0.133886 0.133886 0.0151074"/>
+                  <joint name="shoulder_lift_joint" class="size3"/>
+                  <geom mesh="upperarm_0" material="linkgray" class="visual"/>
+                  <geom mesh="upperarm_1" material="black" class="visual"/>
+                  <geom mesh="upperarm_2" material="jointgray" class="visual"/>
+                  <geom mesh="upperarm_3" material="urblue" class="visual"/>
+                  <geom class="collision" pos="0 -0.04 0" quat="1 1 0 0" size="0.06 0.06"/>
+                  <geom class="collision" size="0.05 0.2" pos="0 0 0.2"/>
                   <body name="forearm_link" gravcomp="1" pos="0 -0.131 0.425">
-                    <inertial
-                      mass="2.275"
-                      pos="0 0 0.196"
-                      diaginertia="0.0311796 0.0311796 0.004095"
-                    />
-                    <joint name="elbow_joint" class="size3_limited" />
-                    <geom mesh="forearm_0" material="urblue" class="visual" />
-                    <geom mesh="forearm_1" material="linkgray" class="visual" />
-                    <geom mesh="forearm_2" material="black" class="visual" />
-                    <geom
-                      mesh="forearm_3"
-                      material="jointgray"
-                      class="visual"
-                    />
-                    <geom
-                      class="collision"
-                      pos="0 0.08 0"
-                      quat="1 1 0 0"
-                      size="0.055 0.06"
-                    />
-                    <geom class="collision" size="0.038 0.19" pos="0 0 0.2" />
-                    <body
-                      name="wrist_1_link"
-                      gravcomp="1"
-                      pos="0 0 0.392"
-                      quat="1 0 1 0"
-                    >
-                      <inertial
-                        mass="1.219"
-                        pos="0 0.127 0"
-                        diaginertia="0.0025599 0.0025599 0.0021942"
-                      />
-                      <joint name="wrist_1_joint" class="size3" />
-                      <geom mesh="wrist1_0" material="black" class="visual" />
-                      <geom mesh="wrist1_1" material="urblue" class="visual" />
-                      <geom
-                        mesh="wrist1_2"
-                        material="jointgray"
-                        class="visual"
-                      />
-                      <geom
-                        class="collision"
-                        pos="0 0.05 0"
-                        quat="1 1 0 0"
-                        size="0.04 0.07"
-                      />
+                    <inertial mass="2.275" pos="0 0 0.196" diaginertia="0.0311796 0.0311796 0.004095"/>
+                    <joint name="elbow_joint" class="size3_limited"/>
+                    <geom mesh="forearm_0" material="urblue" class="visual"/>
+                    <geom mesh="forearm_1" material="linkgray" class="visual"/>
+                    <geom mesh="forearm_2" material="black" class="visual"/>
+                    <geom mesh="forearm_3" material="jointgray" class="visual"/>
+                    <geom class="collision" pos="0 0.08 0" quat="1 1 0 0" size="0.055 0.06"/>
+                    <geom class="collision" size="0.038 0.19" pos="0 0 0.2"/>
+                    <body name="wrist_1_link" gravcomp="1" pos="0 0 0.392" quat="1 0 1 0">
+                      <inertial mass="1.219" pos="0 0.127 0" diaginertia="0.0025599 0.0025599 0.0021942"/>
+                      <joint name="wrist_1_joint" class="size3"/>
+                      <geom mesh="wrist1_0" material="black" class="visual"/>
+                      <geom mesh="wrist1_1" material="urblue" class="visual"/>
+                      <geom mesh="wrist1_2" material="jointgray" class="visual"/>
+                      <geom class="collision" pos="0 0.05 0" quat="1 1 0 0" size="0.04 0.07"/>
                       <body name="wrist_2_link" gravcomp="1" pos="0 0.127 0">
-                        <inertial
-                          mass="1.219"
-                          pos="0 0 0.1"
-                          diaginertia="0.0025599 0.0025599 0.0021942"
-                        />
-                        <joint
-                          name="wrist_2_joint"
-                          axis="0 0 1"
-                          class="size3"
-                        />
-                        <geom mesh="wrist2_0" material="black" class="visual" />
-                        <geom
-                          mesh="wrist2_1"
-                          material="urblue"
-                          class="visual"
-                        />
-                        <geom
-                          mesh="wrist2_2"
-                          material="jointgray"
-                          class="visual"
-                        />
-                        <geom
-                          class="collision"
-                          size="0.04 0.06"
-                          pos="0 0 0.04"
-                        />
-                        <geom
-                          class="collision"
-                          pos="0 0.02 0.1"
-                          quat="1 1 0 0"
-                          size="0.04 0.04"
-                        />
+                        <inertial mass="1.219" pos="0 0 0.1" diaginertia="0.0025599 0.0025599 0.0021942"/>
+                        <joint name="wrist_2_joint" axis="0 0 1" class="size3"/>
+                        <geom mesh="wrist2_0" material="black" class="visual"/>
+                        <geom mesh="wrist2_1" material="urblue" class="visual"/>
+                        <geom mesh="wrist2_2" material="jointgray" class="visual"/>
+                        <geom class="collision" size="0.04 0.06" pos="0 0 0.04"/>
+                        <geom class="collision" pos="0 0.02 0.1" quat="1 1 0 0" size="0.04 0.04"/>
                         <body name="wrist_3_link" gravcomp="1" pos="0 0 0.1">
-                          <inertial
-                            mass="0.1889"
-                            pos="0 0.0771683 0"
-                            quat="1 0 0 1"
-                            diaginertia="0.000132134 9.90863e-05 9.90863e-05"
-                          />
-                          <joint name="wrist_3_joint" class="size3" />
-                          <geom
-                            material="linkgray"
-                            mesh="wrist3"
-                            class="visual"
-                          />
-                          <geom
-                            class="eef_collision"
-                            pos="0 0.08 0"
-                            quat="1 1 0 0"
-                            size="0.04 0.02"
-                          />
-                          <site
-                            name="attachment_site"
-                            pos="0 0.1 0"
-                            quat="-1 1 0 0"
-                          />
+                          <inertial mass="0.1889" pos="0 0.0771683 0" quat="1 0 0 1" diaginertia="0.000132134 9.90863e-05 9.90863e-05"/>
+                          <joint name="wrist_3_joint" class="size3"/>
+                          <geom material="linkgray" mesh="wrist3" class="visual"/>
+                          <geom class="eef_collision" pos="0 0.08 0" quat="1 1 0 0" size="0.04 0.02"/>
+                          <site name="attachment_site" pos="0 0.1 0" quat="-1 1 0 0"/>
                           <!-- Vacuum + end effector attachments -->
                           <body>
                             <!-- site for end effector's f/t sensor right at the base of the gripper -->
-                            <site
-                              name="tcp_fts_sensor"
-                              size="0.01"
-                              pos="0.001 0.1 0"
-                              quat="-1 1 0 0"
-                            />
-                            <body
-                              name="camera_adapter"
-                              gravcomp="1"
-                              pos="0.0 0.1 0"
-                              euler="-1.57 3.14 0"
-                            >
-                              <geom
-                                mesh="camera_adapter"
-                                material="linkgray"
-                                class="visual"
-                              />
+                            <site name="tcp_fts_sensor" size="0.01" pos="0.001 0.1 0" quat="-1 1 0 0"/>
+                            <body name="camera_adapter" gravcomp="1" pos="0.0 0.1 0" euler="-1.57 3.14 0">
+                              <geom mesh="camera_adapter" material="linkgray" class="visual"/>
                             </body>
-                            <camera
-                              name="wrist_camera"
-                              pos="0.0 0.2 0.1"
-                              fovy="58"
-                              mode="fixed"
-                              resolution="1280 720"
-                              euler="1.57 0.0 0.0"
-                            />
-                            <site
-                              name="wrist_camera_optical_frame"
-                              pos="0.0 0.2 0.1"
-                              euler="-1.57 0 0"
-                            />
-                            <body
-                              gravcomp="1"
-                              pos="0.0 0.1 0"
-                              quat="0.5 -0.5 0.5 0.5"
-                            >
-                              <include file="vacuum.xml" />
+                            <camera name="wrist_camera" pos="0.0 0.2 0.1" fovy="58" mode="fixed" resolution="1280 720" euler="1.57 0.0 0.0"/>
+                            <site name="wrist_camera_optical_frame" pos="0.0 0.2 0.1" euler="-1.57 0 0"/>
+                            <body gravcomp="1" pos="0.0 0.1 0" quat="0.5 -0.5 0.5 0.5">
+                              <include file="vacuum.xml"/>
                             </body>
                           </body>
                         </body>
@@ -502,77 +238,27 @@
   </worldbody>
 
   <actuator>
-    <velocity
-      class="size3"
-      name="shoulder_pan_joint"
-      joint="shoulder_pan_joint"
-      kv="500"
-    />
-    <velocity
-      class="size3"
-      name="shoulder_lift_joint"
-      joint="shoulder_lift_joint"
-      kv="500"
-    />
-    <velocity
-      class="size3_limited"
-      name="elbow_joint"
-      joint="elbow_joint"
-      kv="500"
-    />
-    <velocity
-      class="size3"
-      name="wrist_1_joint"
-      joint="wrist_1_joint"
-      kv="500"
-    />
-    <velocity
-      class="size3"
-      name="wrist_2_joint"
-      joint="wrist_2_joint"
-      kv="500"
-    />
-    <velocity
-      class="size3"
-      name="wrist_3_joint"
-      joint="wrist_3_joint"
-      kv="500"
-    />
+    <velocity class="size3" name="shoulder_pan_joint" joint="shoulder_pan_joint" kv="500"/>
+    <velocity class="size3" name="shoulder_lift_joint" joint="shoulder_lift_joint" kv="500"/>
+    <velocity class="size3_limited" name="elbow_joint" joint="elbow_joint" kv="500"/>
+    <velocity class="size3" name="wrist_1_joint" joint="wrist_1_joint" kv="500"/>
+    <velocity class="size3" name="wrist_2_joint" joint="wrist_2_joint" kv="500"/>
+    <velocity class="size3" name="wrist_3_joint" joint="wrist_3_joint" kv="500"/>
   </actuator>
 
   <contact>
-    <exclude body1="base_platform" body2="base" />
+    <exclude body1="base_platform" body2="base"/>
   </contact>
 
   <actuator>
-    <velocity
-      name="front_right_wheel"
-      joint="front_right_wheel"
-      ctrlrange="-15 15"
-      kv="50"
-    />
-    <velocity
-      name="front_left_wheel"
-      joint="front_left_wheel"
-      ctrlrange="-15 15"
-      kv="50"
-    />
-    <velocity
-      name="rear_right_wheel"
-      joint="rear_right_wheel"
-      ctrlrange="-15 15"
-      kv="50"
-    />
-    <velocity
-      name="rear_left_wheel"
-      joint="rear_left_wheel"
-      ctrlrange="-15 15"
-      kv="50"
-    />
+    <velocity name="front_right_wheel" joint="front_right_wheel" ctrlrange="-15 15" kv="50"/>
+    <velocity name="front_left_wheel" joint="front_left_wheel" ctrlrange="-15 15" kv="50"/>
+    <velocity name="rear_right_wheel" joint="rear_right_wheel" ctrlrange="-15 15" kv="50"/>
+    <velocity name="rear_left_wheel" joint="rear_left_wheel" ctrlrange="-15 15" kv="50"/>
   </actuator>
 
   <sensor>
-    <force site="tcp_fts_sensor" />
-    <torque site="tcp_fts_sensor" />
+    <force site="tcp_fts_sensor"/>
+    <torque site="tcp_fts_sensor"/>
   </sensor>
 </mujoco>
diff --git a/src/hangar_sim/description/vacuum.xml b/src/hangar_sim/description/vacuum.xml
index 002a60d6..83a9b252 100644
--- a/src/hangar_sim/description/vacuum.xml
+++ b/src/hangar_sim/description/vacuum.xml
@@ -1,72 +1,18 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco>
-  <body
-    name="vacuum_base"
-    gravcomp="1"
-    pos="0.0 0.0 0.0"
-    quat="0.5 0.0 0.0 -0.5"
-  >
-    <geom
-      class="visual"
-      type="mesh"
-      name="vacuum_base geom"
-      mesh="vacuum_base"
-      material="metal"
-    />
-    <body
-      name="collision_vacuum_base"
-      gravcomp="1"
-      pos="0.0 0.0 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_vacuum_base geom"
-        mesh="collision_vacuum_base"
-      />
+  <body name="vacuum_base" gravcomp="1" pos="0.0 0.0 0.0" quat="0.5 0.0 0.0 -0.5">
+    <geom class="visual" type="mesh" name="vacuum_base geom" mesh="vacuum_base" material="metal"/>
+    <body name="collision_vacuum_base" gravcomp="1" pos="0.0 0.0 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_vacuum_base geom" mesh="collision_vacuum_base"/>
     </body>
-    <body
-      name="collision_vacuum_suction_cups"
-      gravcomp="1"
-      pos="0.0 0.0 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="collision"
-        type="mesh"
-        name="collision_vacuum_suction_cups geom"
-        mesh="collision_vacuum_suction_cups"
-        margin="0.02"
-        gap="0.02"
-      />
-      <site name="suction_cup" type="box" size="0.1 0.1 0.1" />
+    <body name="collision_vacuum_suction_cups" gravcomp="1" pos="0.0 0.0 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="collision" type="mesh" name="collision_vacuum_suction_cups geom" mesh="collision_vacuum_suction_cups" margin="0.02" gap="0.02"/>
+      <site name="suction_cup" type="box" size="0.1 0.1 0.1"/>
     </body>
-    <body
-      name="vacuum_base_top"
-      gravcomp="1"
-      pos="0.0 0.0 0.0"
-      quat="1.0 0.0 0.0 0.0"
-    >
-      <geom
-        class="visual"
-        type="mesh"
-        name="vacuum_base_top geom"
-        mesh="vacuum_base_top"
-        material="dark metal"
-      />
-      <body
-        name="vacuum_suction_cups"
-        gravcomp="1"
-        pos="0.0 0.0 0.0"
-        quat="1.0 0.0 0.0 0.0"
-      >
-        <geom
-          class="visual"
-          type="mesh"
-          name="vacuum_suction_cups geom"
-          mesh="vacuum_suction_cups"
-          material="plastic"
-        />
+    <body name="vacuum_base_top" gravcomp="1" pos="0.0 0.0 0.0" quat="1.0 0.0 0.0 0.0">
+      <geom class="visual" type="mesh" name="vacuum_base_top geom" mesh="vacuum_base_top" material="dark metal"/>
+      <body name="vacuum_suction_cups" gravcomp="1" pos="0.0 0.0 0.0" quat="1.0 0.0 0.0 0.0">
+        <geom class="visual" type="mesh" name="vacuum_suction_cups geom" mesh="vacuum_suction_cups" material="plastic"/>
       </body>
     </body>
   </body>
diff --git a/src/hangar_sim/launch/agent_bridge.launch.xml b/src/hangar_sim/launch/agent_bridge.launch.xml
index bbe87d15..1798e803 100644
--- a/src/hangar_sim/launch/agent_bridge.launch.xml
+++ b/src/hangar_sim/launch/agent_bridge.launch.xml
@@ -1,8 +1,6 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
   <!--  <node-->
   <!--    pkg="hangar_sim"-->
   <!--    exec="odometry_joint_state_publisher.py"-->
diff --git a/src/hangar_sim/moveit_mecanum_drive_controller_plugins.xml b/src/hangar_sim/moveit_mecanum_drive_controller_plugins.xml
index 1874cae6..9a07d78f 100644
--- a/src/hangar_sim/moveit_mecanum_drive_controller_plugins.xml
+++ b/src/hangar_sim/moveit_mecanum_drive_controller_plugins.xml
@@ -1,10 +1,6 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <library path="dummy_controller_handle_plugin">
-  <class
-    name="clearpath_mecanum_drive_controller/MecanumDriveController"
-    type="moveit_studio_ros_control_interface::DummyControllerAllocator"
-    base_class_type="moveit_ros_control_interface::ControllerHandleAllocator"
-  >
+  <class name="clearpath_mecanum_drive_controller/MecanumDriveController" type="moveit_studio_ros_control_interface::DummyControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
     <description>
       A very minimal controller handle allocator plugin to allow the
       MecanumDriveController to be enabled and disabled through the MoveIt
diff --git a/src/hangar_sim/objectives/cartesian_draw_geometry_from_file.xml b/src/hangar_sim/objectives/cartesian_draw_geometry_from_file.xml
index 4e37cb4f..2cfdc5f6 100644
--- a/src/hangar_sim/objectives/cartesian_draw_geometry_from_file.xml
+++ b/src/hangar_sim/objectives/cartesian_draw_geometry_from_file.xml
@@ -1,69 +1,18 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Cartesian Draw Geometry From File">
   <!--//////////-->
-  <BehaviorTree
-    ID="Cartesian Draw Geometry From File"
-    _description="Training example that shows how to load a Cartesian path from a file and execute the path"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Cartesian Draw Geometry From File" _description="Training example that shows how to load a Cartesian path from a file and execute the path" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="LoadPoseStampedVectorFromYaml"
-        output="{pose_stamped_msgs}"
-        file_path="picknik.yaml"
-      />
-      <Action
-        ID="CreateStampedPose"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;1;0"
-        position_xyz="0;0;0.5"
-        reference_frame="grasp_link"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="pose"
-        marker_size="0.100000"
-        pose="{stamped_pose}"
-      />
+      <Action ID="LoadPoseStampedVectorFromYaml" output="{pose_stamped_msgs}" file_path="picknik.yaml"/>
+      <Action ID="CreateStampedPose" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;1;0" position_xyz="0;0;0.5" reference_frame="grasp_link"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{stamped_pose}"/>
       <Control ID="Parallel" failure_count="1" success_count="1">
-        <Action
-          ID="PublishStaticFrame"
-          child_frame_id="local"
-          pose="{stamped_pose}"
-          publish_rate="50"
-        />
+        <Action ID="PublishStaticFrame" child_frame_id="local" pose="{stamped_pose}" publish_rate="50"/>
         <Control ID="Sequence" name="TopLevelSequence">
-          <Action ID="WaitForDuration" delay_duration="1" />
-          <Action
-            ID="PlanCartesianPath"
-            acceleration_scale_factor="1.000000"
-            blending_radius="0.001"
-            debug_solution="{debug_solution}"
-            ik_cartesian_space_density="0.010000"
-            ik_joint_space_density="0.100000"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-            path="{pose_stamped_msgs}"
-            planning_group_name="manipulator"
-            position_only="false"
-            tip_link="grasp_link"
-            trajectory_sampling_rate="100"
-            velocity_scale_factor="1.000000"
-          />
-          <Action
-            ID="VisualizePath"
-            marker_lifetime="0.000000"
-            path="{pose_stamped_msgs}"
-          />
-          <Action
-            ID="ExecuteFollowJointTrajectory"
-            execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-            goal_duration_tolerance="-1.000000"
-            goal_position_tolerance="0.000000"
-            goal_time_tolerance="0.000000"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-          />
+          <Action ID="WaitForDuration" delay_duration="1"/>
+          <Action ID="PlanCartesianPath" acceleration_scale_factor="1.000000" blending_radius="0.001" debug_solution="{debug_solution}" ik_cartesian_space_density="0.010000" ik_joint_space_density="0.100000" joint_trajectory_msg="{joint_trajectory_msg}" path="{pose_stamped_msgs}" planning_group_name="manipulator" position_only="false" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+          <Action ID="VisualizePath" marker_lifetime="0.000000" path="{pose_stamped_msgs}"/>
+          <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
         </Control>
       </Control>
     </Control>
@@ -71,12 +20,12 @@
   <TreeNodesModel>
     <SubTree ID="Cartesian Draw Geometry From File">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Training Examples" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Training Examples"/>
       </MetadataFields>
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Training Examples" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Training Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/cartesian_path_with_collision_checking.xml b/src/hangar_sim/objectives/cartesian_path_with_collision_checking.xml
index e11a7fcd..0a942029 100644
--- a/src/hangar_sim/objectives/cartesian_path_with_collision_checking.xml
+++ b/src/hangar_sim/objectives/cartesian_path_with_collision_checking.xml
@@ -1,92 +1,29 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Cartesian Path with Collision Checking"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Cartesian Path with Collision Checking">
   <!--//////////-->
-  <BehaviorTree
-    ID="Cartesian Path with Collision Checking"
-    _description="An example of planning and executing a straight line path with base and arm motion"
-    _favorite="false"
-  >
+  <BehaviorTree ID="Cartesian Path with Collision Checking" _description="An example of planning and executing a straight line path with base and arm motion" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.0"
-        velocity_scale_factor="1.0"
-        waypoint_name="Look at Plane"
-        keep_orientation_link_names="grasp_link"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;5"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="pose"
-        marker_size="0.100000"
-        pose="{stamped_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{stamped_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <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="{pose_stamped_vector}"
-        planning_group_name="manipulator"
-        position_only="true"
-        tip_link="grasp_link"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="1.000000"
-      />
-      <Action
-        ID="GetCurrentPlanningScene"
-        planning_scene_msg="{planning_scene}"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.0" velocity_scale_factor="1.0" waypoint_name="Look at Plane" keep_orientation_link_names="grasp_link"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;0;5"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{stamped_pose}"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+      <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="{pose_stamped_vector}" planning_group_name="manipulator" position_only="true" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+      <Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene}"/>
       <Control ID="Fallback">
-        <Action
-          ID="ValidateTrajectory"
-          cartesian_space_step="0.020000"
-          debug_solution="{debug_solution}"
-          joint_space_step="0.100000"
-          joint_trajectory_msg="{joint_trajectory_msg}"
-          planning_group_name="manipulator"
-          planning_scene_msg="{planning_scene}"
-        />
-        <Action
-          ID="WaitForUserTrajectoryApproval"
-          solution="{debug_solution}"
-        />
+        <Action ID="ValidateTrajectory" cartesian_space_step="0.020000" debug_solution="{debug_solution}" joint_space_step="0.100000" joint_trajectory_msg="{joint_trajectory_msg}" planning_group_name="manipulator" planning_scene_msg="{planning_scene}"/>
+        <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Cartesian Path with Collision Checking">
       <MetadataFields>
-        <Metadata subcategory="Training Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Training Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
       <MetadataFields>
-        <Metadata subcategory="Training Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Training Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/cartesian_plan_simple_square.xml b/src/hangar_sim/objectives/cartesian_plan_simple_square.xml
index 041d7362..e136338c 100644
--- a/src/hangar_sim/objectives/cartesian_plan_simple_square.xml
+++ b/src/hangar_sim/objectives/cartesian_plan_simple_square.xml
@@ -1,131 +1,32 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Cartesian Plan Simple Square">
   <!--//////////-->
-  <BehaviorTree
-    ID="Cartesian Plan Simple Square"
-    _description="Training example that shows how to create a path from poses and execute the path"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Cartesian Plan Simple Square" _description="Training example that shows how to create a path from poses and execute the path" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_link_names="grasp_link"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.0"
-        velocity_scale_factor="1.0"
-        waypoint_name="Look at Plane"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0.4"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="pose"
-        marker_size="0.100000"
-        pose="{stamped_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{stamped_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;-0.4;0.4"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{stamped_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0.4;-0.4;0.4"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{stamped_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0.4;0;0.4"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{stamped_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0.4"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{stamped_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <Action
-        ID="VisualizePath"
-        marker_lifetime="0.000000"
-        path="{pose_stamped_vector}"
-      />
-      <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="{pose_stamped_vector}"
-        planning_group_name="manipulator"
-        position_only="false"
-        tip_link="grasp_link"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="1.000000"
-      />
-      <Action ID="IsUserAvailable" />
-      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-        goal_duration_tolerance="-1.000000"
-        goal_position_tolerance="0.000000"
-        goal_time_tolerance="0.000000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.05" link_padding="0.0" velocity_scale_factor="1.0" waypoint_name="Look at Plane"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;0;0.4"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{stamped_pose}"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;-0.4;0.4"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0.4;-0.4;0.4"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0.4;0;0.4"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;0;0.4"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+      <Action ID="VisualizePath" marker_lifetime="0.000000" path="{pose_stamped_vector}"/>
+      <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="{pose_stamped_vector}" planning_group_name="manipulator" position_only="false" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+      <Action ID="IsUserAvailable"/>
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Cartesian Plan Simple Square">
       <MetadataFields>
-        <Metadata subcategory="Training Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Training Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/close_gripper.xml b/src/hangar_sim/objectives/close_gripper.xml
index 68e76f1d..416d43f3 100644
--- a/src/hangar_sim/objectives/close_gripper.xml
+++ b/src/hangar_sim/objectives/close_gripper.xml
@@ -1,21 +1,16 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Close Gripper">
-  <BehaviorTree
-    ID="Close Gripper"
-    _description="Close the gripper"
-    _favorite="true"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Close Gripper" _description="Close the gripper" _favorite="true" _subtreeOnly="true">
     <Control ID="Sequence" name="root">
       <!--Not used in this config-->
-      <Action ID="AlwaysFailure" />
+      <Action ID="AlwaysFailure"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Close Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
-        <Metadata runnable="false" />
+        <Metadata subcategory="Grasping"/>
+        <Metadata runnable="false"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/find_and_spray_plane.xml b/src/hangar_sim/objectives/find_and_spray_plane.xml
index 7358939f..60f97ff8 100644
--- a/src/hangar_sim/objectives/find_and_spray_plane.xml
+++ b/src/hangar_sim/objectives/find_and_spray_plane.xml
@@ -1,91 +1,24 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Find and Spray Plane">
   <!--//////////-->
-  <BehaviorTree
-    ID="Find and Spray Plane"
-    _description="Finds the plane then generates and executes a coverage path"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Find and Spray Plane" _description="Finds the plane then generates and executes a coverage path" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.0"
-        velocity_scale_factor="1.0"
-        waypoint_name="Look at Plane"
-        keep_orientation_link_names="grasp_link"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;1"
-      />
-      <SubTree
-        ID="Get Point Cloud Center Pose"
-        _collapsed="true"
-        input_pose="{stamped_pose}"
-        output_pose="{center_pose}"
-      />
-      <Action
-        ID="TransformPose"
-        input_pose="{center_pose}"
-        output_pose="{start_pose}"
-        quaternion_xyzw="0;0;-0.707;0.707"
-        translation_xyz="-1;1;-0.1"
-      />
-      <Action
-        ID="GenerateCoveragePath"
-        vector_of_poses="{pose_stamped_vector}"
-        bottom_right_corner="{start_pose}"
-        stride_distance="0.5"
-        width="0.5"
-        height="5"
-      />
-      <Action
-        ID="VisualizePath"
-        marker_lifetime="0.000000"
-        path="{pose_stamped_vector}"
-      />
-      <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="{pose_stamped_vector}"
-        planning_group_name="manipulator"
-        position_only="false"
-        tip_link="grasp_link"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="1.000000"
-      />
-      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-        goal_duration_tolerance="-1.000000"
-        goal_position_tolerance="0.000000"
-        goal_time_tolerance="0.000000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.0" velocity_scale_factor="1.0" waypoint_name="Look at Plane" keep_orientation_link_names="grasp_link"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;0;1"/>
+      <SubTree ID="Get Point Cloud Center Pose" _collapsed="true" input_pose="{stamped_pose}" output_pose="{center_pose}"/>
+      <Action ID="TransformPose" input_pose="{center_pose}" output_pose="{start_pose}" quaternion_xyzw="0;0;-0.707;0.707" translation_xyz="-1;1;-0.1"/>
+      <Action ID="GenerateCoveragePath" vector_of_poses="{pose_stamped_vector}" bottom_right_corner="{start_pose}" stride_distance="0.5" width="0.5" height="5"/>
+      <Action ID="VisualizePath" marker_lifetime="0.000000" path="{pose_stamped_vector}"/>
+      <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="{pose_stamped_vector}" planning_group_name="manipulator" position_only="false" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Find and Spray Plane">
       <MetadataFields>
-        <Metadata subcategory="Training Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Training Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/generate_grid_pattern_on_airplane.xml b/src/hangar_sim/objectives/generate_grid_pattern_on_airplane.xml
index 3c00ac35..3683842f 100644
--- a/src/hangar_sim/objectives/generate_grid_pattern_on_airplane.xml
+++ b/src/hangar_sim/objectives/generate_grid_pattern_on_airplane.xml
@@ -1,84 +1,22 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Generate Grid Pattern on Airplane">
   <!--//////////-->
-  <BehaviorTree
-    ID="Generate Grid Pattern on Airplane"
-    _description="A training example that plans and executes a coverage path with base and arm motion"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Generate Grid Pattern on Airplane" _description="A training example that plans and executes a coverage path with base and arm motion" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_link_names="grasp_link"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.0"
-        velocity_scale_factor="1.0"
-        waypoint_name="Look at Plane"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;-0.5;1"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="pose"
-        marker_size="0.100000"
-        pose="{stamped_pose}"
-      />
-      <Action
-        ID="GenerateCoveragePath"
-        vector_of_poses="{pose_stamped_vector}"
-        bottom_right_corner="{stamped_pose}"
-        height="0.5"
-        stride_distance="0.1"
-        width="0.5"
-      />
-      <Action
-        ID="VisualizePath"
-        marker_lifetime="0.000000"
-        path="{pose_stamped_vector}"
-      />
-      <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="{pose_stamped_vector}"
-        planning_group_name="manipulator"
-        position_only="false"
-        tip_link="grasp_link"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="1.000000"
-      />
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-        goal_duration_tolerance="-1.000000"
-        goal_position_tolerance="0.000000"
-        goal_time_tolerance="0.000000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.05" link_padding="0.0" velocity_scale_factor="1.0" waypoint_name="Look at Plane"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;-0.5;1"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{stamped_pose}"/>
+      <Action ID="GenerateCoveragePath" vector_of_poses="{pose_stamped_vector}" bottom_right_corner="{stamped_pose}" height="0.5" stride_distance="0.1" width="0.5"/>
+      <Action ID="VisualizePath" marker_lifetime="0.000000" path="{pose_stamped_vector}"/>
+      <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="{pose_stamped_vector}" planning_group_name="manipulator" position_only="false" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Generate Grid Pattern on Airplane">
       <MetadataFields>
-        <Metadata subcategory="Training Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Training Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/get_point_cloud_center_pose.xml b/src/hangar_sim/objectives/get_point_cloud_center_pose.xml
index 7407a18b..e7e63287 100644
--- a/src/hangar_sim/objectives/get_point_cloud_center_pose.xml
+++ b/src/hangar_sim/objectives/get_point_cloud_center_pose.xml
@@ -1,52 +1,22 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Get Point Cloud Center Pose">
   <!--//////////-->
-  <BehaviorTree
-    ID="Get Point Cloud Center Pose"
-    _description="Take a wrist camera snapshot and return the center pose of the point cloud"
-    _favorite="false"
-    input_pose="{stamped_pose}"
-    output_pose="{plane_pose}"
-  >
+  <BehaviorTree ID="Get Point Cloud Center Pose" _description="Take a wrist camera snapshot and return the center pose of the point cloud" _favorite="false" input_pose="{stamped_pose}" output_pose="{plane_pose}">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="GetPointCloud"
-        message_out="{point_cloud}"
-        timeout_sec="5.000000"
-        topic_name="/wrist_camera/points"
-      />
-      <Action
-        ID="CropPointsInBox"
-        crop_box_centroid_pose="{input_pose}"
-        point_cloud="{point_cloud}"
-        point_cloud_cropped="{point_cloud_cropped}"
-        crop_box_size="2;2;2"
-      />
-      <Action
-        ID="SendPointCloudToUI"
-        pcd_topic="/pcd_pointcloud_captures"
-        point_cloud="{point_cloud_cropped}"
-      />
-      <Action
-        ID="GetCentroidFromPointCloud"
-        output_pose="{output_pose}"
-        point_cloud="{point_cloud_cropped}"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_size="1"
-        marker_name="center_pose"
-        pose="{output_pose}"
-      />
+      <Action ID="GetPointCloud" message_out="{point_cloud}" timeout_sec="5.000000" topic_name="/wrist_camera/points"/>
+      <Action ID="CropPointsInBox" crop_box_centroid_pose="{input_pose}" point_cloud="{point_cloud}" point_cloud_cropped="{point_cloud_cropped}" crop_box_size="2;2;2"/>
+      <Action ID="SendPointCloudToUI" pcd_topic="/pcd_pointcloud_captures" point_cloud="{point_cloud_cropped}"/>
+      <Action ID="GetCentroidFromPointCloud" output_pose="{output_pose}" point_cloud="{point_cloud_cropped}"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_size="1" marker_name="center_pose" pose="{output_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Get Point Cloud Center Pose">
-      <inout_port name="input_pose" default="{stamped_pose}" />
-      <inout_port name="output_pose" default="{plane_pose}" />
+      <inout_port name="input_pose" default="{stamped_pose}"/>
+      <inout_port name="output_pose" default="{plane_pose}"/>
       <MetadataFields>
-        <Metadata subcategory="Training Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Training Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/interpolate_to_joint_state.xml b/src/hangar_sim/objectives/interpolate_to_joint_state.xml
index 868d9932..48a28c37 100644
--- a/src/hangar_sim/objectives/interpolate_to_joint_state.xml
+++ b/src/hangar_sim/objectives/interpolate_to_joint_state.xml
@@ -1,45 +1,20 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Interpolate to Joint State">
-  <BehaviorTree
-    ID="Interpolate to Joint State"
-    _description="Move to a specified joint state using joint interpolation"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Interpolate to Joint State" _description="Move to a specified joint state using joint interpolation" _subtreeOnly="true">
     <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="interpolate_to_joint_state"
-        controller_names="{controller_names}"
-        task="{interpolate_to_waypoint_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}" />
-      <Action
-        ID="SetupMTCInterpolateToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCInterpolateToJointState_First"
-        planning_group_name="manipulator"
-        task="{interpolate_to_waypoint_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{interpolate_to_waypoint_solution}"
-        task="{interpolate_to_waypoint_task}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{interpolate_to_waypoint_solution}"
-      />
+      <Action ID="InitializeMTCTask" task_id="interpolate_to_joint_state" controller_names="{controller_names}" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="SetupMTCInterpolateToJointState" joint_state="{target_joint_state}" name="SetupMTCInterpolateToJointState_First" planning_group_name="manipulator" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="PlanMTCTask" solution="{interpolate_to_waypoint_solution}" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="ExecuteMTCTask" solution="{interpolate_to_waypoint_solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Interpolate to Joint State">
-      <input_port name="target_joint_state" default="{target_joint_state}" />
-      <input_port
-        name="controller_names"
-        default="/joint_trajectory_controller"
-      />
+      <input_port name="target_joint_state" default="{target_joint_state}"/>
+      <input_port name="controller_names" default="/joint_trajectory_controller"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/mobile_scan_and_plan.xml b/src/hangar_sim/objectives/mobile_scan_and_plan.xml
index f25c1753..ce800c6e 100644
--- a/src/hangar_sim/objectives/mobile_scan_and_plan.xml
+++ b/src/hangar_sim/objectives/mobile_scan_and_plan.xml
@@ -1,85 +1,23 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Mobile Scan and Plan">
   <!--//////////-->
-  <BehaviorTree
-    ID="Mobile Scan and Plan"
-    _description="An example of computing and executing a coverage path for spraying the plane"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Mobile Scan and Plan" _description="An example of computing and executing a coverage path for spraying the plane" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.0"
-        velocity_scale_factor="1.0"
-        waypoint_name="Look at Plane"
-        keep_orientation_link_names="grasp_link"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;.707;.707"
-        position_xyz="1;-0.5;1"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="pose"
-        marker_size="0.100000"
-        pose="{stamped_pose}"
-      />
-      <Action
-        ID="GenerateCoveragePath"
-        vector_of_poses="{pose_stamped_vector}"
-        bottom_right_corner="{stamped_pose}"
-        stride_distance="0.5"
-        width="0.6"
-        height="5"
-      />
-      <Action
-        ID="VisualizePath"
-        marker_lifetime="0.000000"
-        path="{pose_stamped_vector}"
-      />
-      <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="{pose_stamped_vector}"
-        planning_group_name="manipulator"
-        position_only="false"
-        tip_link="grasp_link"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="1.000000"
-      />
-      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-        goal_duration_tolerance="-1.000000"
-        goal_position_tolerance="0.000000"
-        goal_time_tolerance="0.000000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.0" velocity_scale_factor="1.0" waypoint_name="Look at Plane" keep_orientation_link_names="grasp_link"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;.707;.707" position_xyz="1;-0.5;1"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{stamped_pose}"/>
+      <Action ID="GenerateCoveragePath" vector_of_poses="{pose_stamped_vector}" bottom_right_corner="{stamped_pose}" stride_distance="0.5" width="0.6" height="5"/>
+      <Action ID="VisualizePath" marker_lifetime="0.000000" path="{pose_stamped_vector}"/>
+      <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="{pose_stamped_vector}" planning_group_name="manipulator" position_only="false" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Mobile Scan and Plan">
       <MetadataFields>
-        <Metadata subcategory="Training Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Training Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/move_boxes_to_loading_zone.xml b/src/hangar_sim/objectives/move_boxes_to_loading_zone.xml
index f1649df0..90dfe508 100644
--- a/src/hangar_sim/objectives/move_boxes_to_loading_zone.xml
+++ b/src/hangar_sim/objectives/move_boxes_to_loading_zone.xml
@@ -1,51 +1,17 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move Boxes to Loading Zone">
   <!--//////////-->
-  <BehaviorTree
-    ID="Move Boxes to Loading Zone"
-    _description="Move all of the boxes into the loading zero suing ML perception."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Move Boxes to Loading Zone" _description="Move all of the boxes into the loading zero suing ML perception." _favorite="true">
     <Control ID="Sequence">
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="world"
-        stamped_pose="{place_pose}"
-        orientation_xyzw="0;1.0;0;0"
-        position_xyz="-4.5;8.4;0.5"
-      />
+      <Action ID="CreateStampedPose" reference_frame="world" stamped_pose="{place_pose}" orientation_xyzw="0;1.0;0;0" position_xyz="-4.5;8.4;0.5"/>
       <Decorator ID="RetryUntilSuccessful" num_attempts="1000">
         <Decorator ID="ForceFailure">
           <Control ID="Sequence">
             <Control ID="Fallback">
-              <SubTree
-                ID="Move Boxes to Loading Zone Start from Waypoint"
-                waypoint_name="View Boxes"
-                _collapsed="true"
-                threshold="0.25"
-                prompt="a small brown box"
-                negative_prompts="yellow ladder;"
-                prompts="small boxes"
-                place_pose="{place_pose}"
-                erosion_size="10"
-              />
-              <SubTree
-                ID="Move Boxes to Loading Zone Start from Waypoint"
-                waypoint_name="View Boxes 2"
-                place_pose="{place_pose}"
-                threshold="0.4"
-                _collapsed="true"
-                prompts="small boxes"
-                negative_prompts="gray;gray;rails"
-                erosion_size="2"
-              />
+              <SubTree ID="Move Boxes to Loading Zone Start from Waypoint" waypoint_name="View Boxes" _collapsed="true" threshold="0.25" prompt="a small brown box" negative_prompts="yellow ladder;" prompts="small boxes" place_pose="{place_pose}" erosion_size="10"/>
+              <SubTree ID="Move Boxes to Loading Zone Start from Waypoint" waypoint_name="View Boxes 2" place_pose="{place_pose}" threshold="0.4" _collapsed="true" prompts="small boxes" negative_prompts="gray;gray;rails" erosion_size="2"/>
             </Control>
-            <Action
-              ID="TransformPose"
-              input_pose="{place_pose}"
-              output_pose="{place_pose}"
-              translation_xyz="0.4;0;0"
-            />
+            <Action ID="TransformPose" input_pose="{place_pose}" output_pose="{place_pose}" translation_xyz="0.4;0;0"/>
           </Control>
         </Decorator>
       </Decorator>
@@ -54,8 +20,8 @@
   <TreeNodesModel>
     <SubTree ID="Move Boxes to Loading Zone">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml b/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
index bf79b603..03b19f31 100644
--- a/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
+++ b/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
@@ -1,300 +1,65 @@
-<?xml version="1.0" encoding="utf-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Move Boxes to Loading Zone Start from Waypoint"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Move Boxes to Loading Zone Start from Waypoint">
   <!--//////////-->
-  <BehaviorTree
-    ID="Move Boxes to Loading Zone Start from Waypoint"
-    _description="Move boxes to loading zone from waypoint"
-    _subtreeOnly="true"
-    waypoint_name="View Boxes"
-    place_pose="{place_pose}"
-    threshold="{threshold}"
-    prompt="{prompt}"
-    negative_prompts="{negative_prompts}"
-    prompts="{prompts}"
-    erosion_size="0"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Move Boxes to Loading Zone Start from Waypoint" _description="Move boxes to loading zone from waypoint" _subtreeOnly="true" waypoint_name="View Boxes" place_pose="{place_pose}" threshold="{threshold}" prompt="{prompt}" negative_prompts="{negative_prompts}" prompts="{prompts}" erosion_size="0" _favorite="true">
     <Control ID="Sequence">
-      <Action
-        ID="MoveGripperAction"
-        position="0.0"
-        timeout="10.000000"
-        gripper_command_action_name="/vacuum_gripper/gripper_cmd"
-      />
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.0"
-        velocity_scale_factor="1.0"
-        waypoint_name="{waypoint_name}"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree ID="Clear Snapshot" _collapsed="false" />
+      <Action ID="MoveGripperAction" position="0.0" timeout="10.000000" gripper_command_action_name="/vacuum_gripper/gripper_cmd"/>
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.0" velocity_scale_factor="1.0" waypoint_name="{waypoint_name}" keep_orientation_link_names="grasp_link"/>
+      <SubTree ID="Clear Snapshot" _collapsed="false"/>
       <Decorator ID="Delay" delay_msec="2000">
-        <Action
-          ID="GetPointCloud"
-          topic_name="/wrist_camera/points"
-          timeout_sec="5.000000"
-          message_out="{point_cloud}"
-        />
+        <Action ID="GetPointCloud" topic_name="/wrist_camera/points" timeout_sec="5.000000" message_out="{point_cloud}"/>
       </Decorator>
-      <Action
-        ID="SendPointCloudToUI"
-        pcd_topic="/pcd_pointcloud_captures"
-        point_cloud="{point_cloud}"
-      />
-      <Action
-        ID="TransformPointCloudFrame"
-        input_cloud="{point_cloud}"
-        target_frame="world"
-        output_cloud="{point_cloud}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="world"
-        stamped_pose="{stamped_pose}"
-        position_xyz="0;0;2.05"
-      />
-      <Action
-        ID="CropPointsInBox"
-        crop_box_centroid_pose="{stamped_pose}"
-        point_cloud="{point_cloud}"
-        point_cloud_cropped="{point_cloud_cropped}"
-        crop_box_size="40;40;4"
-      />
-      <Action
-        ID="UpdatePlanningSceneService"
-        point_cloud="{point_cloud_cropped}"
-        point_cloud_service="/point_cloud_service"
-      />
-      <Action
-        ID="GetImage"
-        timeout_sec="5.000000"
-        topic_name="/wrist_camera/color"
-        message_out="{image}"
-      />
-      <Action
-        ID="GetCameraInfo"
-        timeout_sec="5.000000"
-        topic_name="/wrist_camera/camera_info"
-        message_out="{camera_info}"
-      />
+      <Action ID="SendPointCloudToUI" pcd_topic="/pcd_pointcloud_captures" point_cloud="{point_cloud}"/>
+      <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" target_frame="world" output_cloud="{point_cloud}"/>
+      <Action ID="CreateStampedPose" reference_frame="world" stamped_pose="{stamped_pose}" position_xyz="0;0;2.05"/>
+      <Action ID="CropPointsInBox" crop_box_centroid_pose="{stamped_pose}" point_cloud="{point_cloud}" point_cloud_cropped="{point_cloud_cropped}" crop_box_size="40;40;4"/>
+      <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud_cropped}" point_cloud_service="/point_cloud_service"/>
+      <Action ID="GetImage" timeout_sec="5.000000" topic_name="/wrist_camera/color" message_out="{image}"/>
+      <Action ID="GetCameraInfo" timeout_sec="5.000000" topic_name="/wrist_camera/camera_info" message_out="{camera_info}"/>
       <Control ID="Sequence">
-        <Action
-          ID="GetMasks2DFromTextQuery"
-          image="{image}"
-          masks2d="{clip_masks2d}"
-          threshold="{threshold}"
-          clipseg_model_path="models/clipseg.onnx"
-          clip_model_path="models/clip.onnx"
-          model_package="lab_sim"
-          prompts="{prompts}"
-          negative_prompts="{negative_prompts}"
-          erosion_size="{erosion_size}"
-        />
-        <Action
-          ID="PublishMask2D"
-          image="{image}"
-          masks="{clip_masks2d}"
-          masks_visualization_topic="/masks_visualization"
-        />
+        <Action ID="GetMasks2DFromTextQuery" image="{image}" masks2d="{clip_masks2d}" threshold="{threshold}" clipseg_model_path="models/clipseg.onnx" clip_model_path="models/clip.onnx" model_package="lab_sim" prompts="{prompts}" negative_prompts="{negative_prompts}" erosion_size="{erosion_size}"/>
+        <Action ID="PublishMask2D" image="{image}" masks="{clip_masks2d}" masks_visualization_topic="/masks_visualization"/>
         <Decorator ID="Inverter">
-          <Decorator
-            ID="ForEach"
-            vector_in="{clip_masks2d}"
-            index="{index}"
-            out="{clip_mask2d}"
-          >
+          <Decorator ID="ForEach" vector_in="{clip_masks2d}" index="{index}" out="{clip_mask2d}">
             <Decorator ID="Inverter">
               <Control ID="Sequence">
-                <Action
-                  ID="GetCenterFromMask2D"
-                  mask="{clip_mask2d}"
-                  center="{pixel_coords}"
-                />
-                <Action
-                  ID="GetMasks2DFromPointQuery"
-                  image="{image}"
-                  pixel_coords="{pixel_coords}"
-                  masks2d="{masks2d}"
-                  model_package="lab_sim"
-                  decoder_model_path="models/decoder.onnx"
-                  encoder_model_path="models/sam2_hiera_large_encoder.onnx"
-                />
-                <Action
-                  ID="PublishMask2D"
-                  image="{image}"
-                  masks="{masks2d}"
-                  masks_visualization_topic="/masks_visualization"
-                />
-                <Action
-                  ID="GetMasks3DFromMasks2D"
-                  camera_info="{camera_info}"
-                  masks2d="{masks2d}"
-                  point_cloud="{point_cloud_cropped}"
-                  masks3d="{masks3d}"
-                />
-                <Decorator
-                  ID="ForEach"
-                  vector_in="{masks3d}"
-                  index="{index}"
-                  out="{mask3d}"
-                >
+                <Action ID="GetCenterFromMask2D" mask="{clip_mask2d}" center="{pixel_coords}"/>
+                <Action ID="GetMasks2DFromPointQuery" image="{image}" pixel_coords="{pixel_coords}" masks2d="{masks2d}" model_package="lab_sim" decoder_model_path="models/decoder.onnx" encoder_model_path="models/sam2_hiera_large_encoder.onnx"/>
+                <Action ID="PublishMask2D" image="{image}" masks="{masks2d}" masks_visualization_topic="/masks_visualization"/>
+                <Action ID="GetMasks3DFromMasks2D" camera_info="{camera_info}" masks2d="{masks2d}" point_cloud="{point_cloud_cropped}" masks3d="{masks3d}"/>
+                <Decorator ID="ForEach" vector_in="{masks3d}" index="{index}" out="{mask3d}">
                   <Control ID="Sequence">
-                    <Action
-                      ID="GetPointCloudFromMask3D"
-                      mask3d="{mask3d}"
-                      point_cloud="{point_cloud_cropped}"
-                      point_cloud_fragment="{point_cloud_fragment}"
-                    />
-                    <Action
-                      ID="SendPointCloudToUI"
-                      pcd_topic="/pcd_pointcloud_captures"
-                      point_cloud="{point_cloud_fragment}"
-                    />
+                    <Action ID="GetPointCloudFromMask3D" mask3d="{mask3d}" point_cloud="{point_cloud_cropped}" point_cloud_fragment="{point_cloud_fragment}"/>
+                    <Action ID="SendPointCloudToUI" pcd_topic="/pcd_pointcloud_captures" point_cloud="{point_cloud_fragment}"/>
                   </Control>
                 </Decorator>
-                <Action
-                  ID="GetGraspableObjectsFromMasks3D"
-                  base_frame="world"
-                  masks3d="{masks3d}"
-                  minimum_face_area="0.0003"
-                  plane_inlier_threshold="0.01"
-                  point_cloud="{point_cloud_cropped}"
-                  graspable_objects="{graspable_objects}"
-                  face_separation_threshold="0.2"
-                />
-                <Decorator
-                  ID="ForEach"
-                  vector_in="{graspable_objects}"
-                  index="{index}"
-                  out="{graspable_object}"
-                >
+                <Action ID="GetGraspableObjectsFromMasks3D" base_frame="world" masks3d="{masks3d}" minimum_face_area="0.0003" plane_inlier_threshold="0.01" point_cloud="{point_cloud_cropped}" graspable_objects="{graspable_objects}" face_separation_threshold="0.2"/>
+                <Decorator ID="ForEach" vector_in="{graspable_objects}" index="{index}" out="{graspable_object}">
                   <Control ID="Sequence">
-                    <Action
-                      ID="LoadObjectiveParameters"
-                      parameters="{parameters}"
-                      config_file_name="box_picking_vacuum_config.yaml"
-                    />
-                    <Action
-                      ID="InitializeMTCTask"
-                      controller_names="/joint_trajectory_controller"
-                      task="{task}"
-                      task_id=""
-                    />
-                    <Action
-                      ID="SetupMTCCurrentState"
-                      skip_collision_check="false"
-                      task="{task}"
-                    />
-                    <Action
-                      ID="SetupMTCUpdateGroupCollisionRule"
-                      allow_collision="true"
-                      group_name="gripper"
-                      object_name="&lt;octomap&gt;"
-                      task="{task}"
-                    />
-                    <Action
-                      ID="SetupMTCApproachGrasp"
-                      parameters="{parameters}"
-                      target_object="{graspable_object}"
-                      monitored_stage="{monitored_stage}"
-                      task="{task}"
-                    />
-                    <Action
-                      ID="SetupMTCGenerateVacuumGrasps"
-                      parameters="{parameters}"
-                      target_object="{graspable_object}"
-                      monitored_stage="{monitored_stage}"
-                      task="{task}"
-                    />
+                    <Action ID="LoadObjectiveParameters" parameters="{parameters}" config_file_name="box_picking_vacuum_config.yaml"/>
+                    <Action ID="InitializeMTCTask" controller_names="/joint_trajectory_controller" task="{task}" task_id=""/>
+                    <Action ID="SetupMTCCurrentState" skip_collision_check="false" task="{task}"/>
+                    <Action ID="SetupMTCUpdateGroupCollisionRule" allow_collision="true" group_name="gripper" object_name="&lt;octomap&gt;" task="{task}"/>
+                    <Action ID="SetupMTCApproachGrasp" parameters="{parameters}" target_object="{graspable_object}" monitored_stage="{monitored_stage}" task="{task}"/>
+                    <Action ID="SetupMTCGenerateVacuumGrasps" parameters="{parameters}" target_object="{graspable_object}" monitored_stage="{monitored_stage}" task="{task}"/>
                     <Control ID="Parallel" success_count="2" failure_count="1">
                       <Control ID="Sequence">
-                        <Action
-                          ID="PlanMTCTask"
-                          solution="{solution}"
-                          task="{task}"
-                        />
-                        <Action
-                          ID="ExecuteMTCTask"
-                          goal_duration_tolerance="-1.000000"
-                          solution="{solution}"
-                        />
+                        <Action ID="PlanMTCTask" solution="{solution}" task="{task}"/>
+                        <Action ID="ExecuteMTCTask" goal_duration_tolerance="-1.000000" solution="{solution}"/>
                       </Control>
                       <Control ID="Sequence" name="TopLevelSequence">
-                        <Action
-                          ID="LoadPointCloudFromFile"
-                          color="0;255;0"
-                          file_path="box.stl"
-                          point_cloud="{box_mesh}"
-                          frame_id="world"
-                          num_sampled_points="50000"
-                          scale="1.000000"
-                        />
-                        <Action
-                          ID="GetCentroidFromPointCloud"
-                          point_cloud="{point_cloud_fragment}"
-                          output_pose="{point_cloud_fragment_center}"
-                        />
-                        <Action
-                          ID="TransformPointCloud"
-                          input_cloud="{box_mesh}"
-                          transform_pose="{point_cloud_fragment_center}"
-                          output_cloud="{box_mesh}"
-                        />
-                        <Action
-                          ID="RegisterPointClouds"
-                          target_point_cloud="{point_cloud_fragment}"
-                          target_pose_in_base_frame="{target_pose}"
-                          base_point_cloud="{box_mesh}"
-                          max_correspondence_distance="0.05"
-                          max_iterations="40"
-                        />
-                        <Action
-                          ID="TransformPointCloud"
-                          input_cloud="{box_mesh}"
-                          transform_pose="{target_pose}"
-                          output_cloud="{icp_cloud}"
-                        />
-                        <Action
-                          ID="TransformPointCloudFrame"
-                          input_cloud="{icp_cloud}"
-                          output_cloud="{icp_cloud}"
-                          target_frame="world"
-                        />
-                        <Action
-                          ID="SendPointCloudToUI"
-                          pcd_topic="/pcd_pointcloud_captures"
-                          point_cloud="{icp_cloud}"
-                        />
+                        <Action ID="LoadPointCloudFromFile" color="0;255;0" file_path="box.stl" point_cloud="{box_mesh}" frame_id="world" num_sampled_points="50000" scale="1.000000"/>
+                        <Action ID="GetCentroidFromPointCloud" point_cloud="{point_cloud_fragment}" output_pose="{point_cloud_fragment_center}"/>
+                        <Action ID="TransformPointCloud" input_cloud="{box_mesh}" transform_pose="{point_cloud_fragment_center}" output_cloud="{box_mesh}"/>
+                        <Action ID="RegisterPointClouds" target_point_cloud="{point_cloud_fragment}" target_pose_in_base_frame="{target_pose}" base_point_cloud="{box_mesh}" max_correspondence_distance="0.05" max_iterations="40"/>
+                        <Action ID="TransformPointCloud" input_cloud="{box_mesh}" transform_pose="{target_pose}" output_cloud="{icp_cloud}"/>
+                        <Action ID="TransformPointCloudFrame" input_cloud="{icp_cloud}" output_cloud="{icp_cloud}" target_frame="world"/>
+                        <Action ID="SendPointCloudToUI" pcd_topic="/pcd_pointcloud_captures" point_cloud="{icp_cloud}"/>
                       </Control>
                     </Control>
-                    <Action
-                      ID="MoveGripperAction"
-                      timeout="10.000000"
-                      gripper_command_action_name="/vacuum_gripper/gripper_cmd"
-                      position="1.0"
-                    />
-                    <SubTree
-                      ID="Move to Pose No Preview"
-                      _collapsed="true"
-                      target_pose="{place_pose}"
-                    />
-                    <Action
-                      ID="MoveGripperAction"
-                      position="0.0"
-                      timeout="10.000000"
-                      gripper_command_action_name="/vacuum_gripper/gripper_cmd"
-                    />
+                    <Action ID="MoveGripperAction" timeout="10.000000" gripper_command_action_name="/vacuum_gripper/gripper_cmd" position="1.0"/>
+                    <SubTree ID="Move to Pose No Preview" _collapsed="true" target_pose="{place_pose}"/>
+                    <Action ID="MoveGripperAction" position="0.0" timeout="10.000000" gripper_command_action_name="/vacuum_gripper/gripper_cmd"/>
                   </Control>
                 </Decorator>
               </Control>
@@ -306,16 +71,16 @@
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move Boxes to Loading Zone Start from Waypoint">
-      <input_port name="erosion_size" default="0" />
-      <input_port name="negative_prompts" default="{negative_prompts}" />
-      <input_port name="place_pose" default="{place_pose}" />
-      <inout_port name="prompt" default="{prompt}" />
-      <input_port name="prompts" default="{prompts}" />
-      <input_port name="threshold" default="{threshold}" />
-      <input_port name="waypoint_name" default="View Boxes" />
+      <input_port name="erosion_size" default="0"/>
+      <input_port name="negative_prompts" default="{negative_prompts}"/>
+      <input_port name="place_pose" default="{place_pose}"/>
+      <inout_port name="prompt" default="{prompt}"/>
+      <input_port name="prompts" default="{prompts}"/>
+      <input_port name="threshold" default="{threshold}"/>
+      <input_port name="waypoint_name" default="View Boxes"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/move_forward_2m.xml b/src/hangar_sim/objectives/move_forward_2m.xml
index 45b8cede..a232668d 100644
--- a/src/hangar_sim/objectives/move_forward_2m.xml
+++ b/src/hangar_sim/objectives/move_forward_2m.xml
@@ -1,40 +1,18 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move Forward 2m">
   <!--//////////-->
-  <BehaviorTree
-    ID="Move Forward 2m"
-    _description="A training example that moves to a 3D pose using base and arm motion"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Move Forward 2m" _description="A training example that moves to a 3D pose using base and arm motion" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;2"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="pose"
-        marker_size="0.100000"
-        pose="{stamped_pose}"
-      />
-      <SubTree
-        ID="Move to a StampedPose"
-        _collapsed="true"
-        controller_names="/joint_trajectory_controller"
-        target_pose="{stamped_pose}"
-      />
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;0;2"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{stamped_pose}"/>
+      <SubTree ID="Move to a StampedPose" _collapsed="true" controller_names="/joint_trajectory_controller" target_pose="{stamped_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move Forward 2m">
       <MetadataFields>
-        <Metadata subcategory="Training Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Training Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/move_to_a_stampedpose.xml b/src/hangar_sim/objectives/move_to_a_stampedpose.xml
index e2acbc4c..2895bdbe 100644
--- a/src/hangar_sim/objectives/move_to_a_stampedpose.xml
+++ b/src/hangar_sim/objectives/move_to_a_stampedpose.xml
@@ -1,63 +1,28 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to a StampedPose">
-  <BehaviorTree
-    ID="Move to a StampedPose"
-    _description="Uses inverse kinematics to move the robot tip to a given pose"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Move to a StampedPose" _description="Uses inverse kinematics to move the robot tip to a given pose" _subtreeOnly="true">
     <Control ID="Fallback" name="root">
       <Control ID="Sequence">
-        <Action
-          ID="InitializeMTCTask"
-          task_id="move_to_pose"
-          controller_names="{controller_names}"
-          task="{move_to_pose_task}"
-        />
-        <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
-        <Action
-          ID="SetupMTCPlanToPose"
-          ik_frame="grasp_link"
-          planning_group_name="manipulator"
-          target_pose="{target_pose}"
-          task="{move_to_pose_task}"
-        />
-        <Action
-          ID="PlanMTCTask"
-          solution="{move_to_pose_solution}"
-          task="{move_to_pose_task}"
-        />
-        <SubTree
-          ID="Wait for Trajectory Approval if User Available"
-          solution="{move_to_pose_solution}"
-        />
-        <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
-        <Action
-          ID="PublishEmpty"
-          topic="/studio_ui/motion_ended"
-          queue_size="1"
-          use_best_effort="false"
-        />
+        <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="{controller_names}" task="{move_to_pose_task}"/>
+        <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
+        <Action ID="SetupMTCPlanToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}"/>
+        <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+        <SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
+        <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}"/>
+        <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
       </Control>
       <Control ID="Sequence">
-        <Action
-          ID="PublishEmpty"
-          topic="/studio_ui/motion_ended"
-          queue_size="1"
-          use_best_effort="false"
-        />
-        <Action ID="AlwaysFailure" />
+        <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+        <Action ID="AlwaysFailure"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to a StampedPose">
-      <input_port
-        name="controller_names"
-        default="/joint_trajectory_controller"
-      />
-      <input_port name="target_pose" default="{stamped_pose}" />
+      <input_port name="controller_names" default="/joint_trajectory_controller"/>
+      <input_port name="target_pose" default="{stamped_pose}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/move_to_arm_upright.xml b/src/hangar_sim/objectives/move_to_arm_upright.xml
index 8cb5d42f..18197867 100644
--- a/src/hangar_sim/objectives/move_to_arm_upright.xml
+++ b/src/hangar_sim/objectives/move_to_arm_upright.xml
@@ -1,34 +1,16 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Arm Upright">
   <!--//////////-->
-  <BehaviorTree
-    ID="Move to Arm Upright"
-    _description="Move the arm to an upright position"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Move to Arm Upright" _description="Move the arm to an upright position" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_link_names="grasp_link"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.0"
-        velocity_scale_factor="1.0"
-        waypoint_name="Arm upright"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.05" link_padding="0.0" velocity_scale_factor="1.0" waypoint_name="Arm upright"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Arm Upright">
       <MetadataFields>
-        <Metadata subcategory="Training Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Training Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/move_to_look_at_plane.xml b/src/hangar_sim/objectives/move_to_look_at_plane.xml
index 6fbb4943..af642d9b 100644
--- a/src/hangar_sim/objectives/move_to_look_at_plane.xml
+++ b/src/hangar_sim/objectives/move_to_look_at_plane.xml
@@ -1,34 +1,16 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Look at Plane">
   <!--//////////-->
-  <BehaviorTree
-    ID="Move to Look at Plane"
-    _description="Move the base an arm close to the plane"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Move to Look at Plane" _description="Move the base an arm close to the plane" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_link_names="grasp_link"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.0"
-        velocity_scale_factor="1.0"
-        waypoint_name="Look at Plane"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.05" link_padding="0.0" velocity_scale_factor="1.0" waypoint_name="Look at Plane"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Look at Plane">
       <MetadataFields>
-        <Metadata subcategory="Training Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Training Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/move_to_pose.xml b/src/hangar_sim/objectives/move_to_pose.xml
index 1b718311..3c0e13e2 100644
--- a/src/hangar_sim/objectives/move_to_pose.xml
+++ b/src/hangar_sim/objectives/move_to_pose.xml
@@ -1,60 +1,20 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Pose">
-  <BehaviorTree
-    ID="Move to Pose"
-    _description="Plan and execute motion to a Cartesian coordinate pose in a ROS message type, using inverse kinematics."
-    _subtreeOnly="true"
-    target_pose="{target_pose}"
-  >
+  <BehaviorTree ID="Move to Pose" _description="Plan and execute motion to a Cartesian coordinate pose in a ROS message type, using inverse kinematics." _subtreeOnly="true" target_pose="{target_pose}">
     <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_pose"
-        controller_names="/joint_trajectory_controller"
-        task="{move_to_pose_task}"
-      />
-      <Action
-        ID="SetupMTCCurrentState"
-        task="{move_to_pose_task}"
-        skip_collision_check="false"
-      />
-      <Action
-        ID="SetupMTCPlanToPose"
-        ik_frame="grasp_link"
-        planning_group_name="manipulator"
-        target_pose="{target_pose}"
-        task="{move_to_pose_task}"
-        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="5000"
-        monitored_stage="current state"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="1.000000"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_pose_solution}"
-        task="{move_to_pose_task}"
-      />
-      <SubTree
-        ID="Wait for Trajectory Approval if User Available"
-        solution="{move_to_pose_solution}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{move_to_pose_solution}"
-        goal_duration_tolerance="-1.000000"
-      />
+      <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" skip_collision_check="false"/>
+      <Action ID="SetupMTCPlanToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" 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="5000" monitored_stage="current state" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+      <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+      <SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" goal_duration_tolerance="-1.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Pose">
-      <input_port name="target_pose" default="{target_pose}" />
+      <input_port name="target_pose" default="{target_pose}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/move_to_pose_no_preview.xml b/src/hangar_sim/objectives/move_to_pose_no_preview.xml
index 845d9307..afafaf46 100644
--- a/src/hangar_sim/objectives/move_to_pose_no_preview.xml
+++ b/src/hangar_sim/objectives/move_to_pose_no_preview.xml
@@ -1,57 +1,20 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Pose No Preview">
-  <BehaviorTree
-    ID="Move to Pose No Preview"
-    _description="Plan and execute motion to a Cartesian coordinate pose in a ROS message type, using inverse kinematics."
-    _subtreeOnly="true"
-    target_pose="{target_pose}"
-  >
+  <BehaviorTree ID="Move to Pose No Preview" _description="Plan and execute motion to a Cartesian coordinate pose in a ROS message type, using inverse kinematics." _subtreeOnly="true" target_pose="{target_pose}">
     <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_pose"
-        controller_names="/joint_trajectory_controller"
-        task="{move_to_pose_task}"
-      />
-      <Action
-        ID="SetupMTCCurrentState"
-        task="{move_to_pose_task}"
-        skip_collision_check="false"
-      />
-      <Action
-        ID="SetupMTCPlanToPose"
-        ik_frame="grasp_link"
-        planning_group_name="manipulator"
-        target_pose="{target_pose}"
-        task="{move_to_pose_task}"
-        monitored_stage="current state"
-        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="5000"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="1.000000"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_pose_solution}"
-        task="{move_to_pose_task}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{move_to_pose_solution}"
-        goal_duration_tolerance="-1.000000"
-      />
+      <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" skip_collision_check="false"/>
+      <Action ID="SetupMTCPlanToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" monitored_stage="current state" 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="5000" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+      <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" goal_duration_tolerance="-1.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Pose No Preview">
-      <input_port name="target_pose" default="{target_pose}" />
+      <input_port name="target_pose" default="{target_pose}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
-        <Metadata runnable="false" />
+        <Metadata subcategory="Motion - Execute"/>
+        <Metadata runnable="false"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/navigate_to_clicked_point.xml b/src/hangar_sim/objectives/navigate_to_clicked_point.xml
index 1712bf27..ec7d2441 100644
--- a/src/hangar_sim/objectives/navigate_to_clicked_point.xml
+++ b/src/hangar_sim/objectives/navigate_to_clicked_point.xml
@@ -1,58 +1,21 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Navigate to Clicked Point">
   <!--//////////-->
-  <BehaviorTree
-    ID="Navigate to Clicked Point"
-    _description="Navigate to a point clicked by the user in the UI"
-    _favorite="true"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Navigate to Clicked Point" _description="Navigate to a point clicked by the user in the UI" _favorite="true" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="base_link"
-        stamped_pose="{start}"
-      />
-      <Action
-        ID="ActivateControllers"
-        controller_names="platform_velocity_controller_nav2"
-      />
-      <Action
-        ID="GetPoseFromUser"
-        is_normal="true"
-        pose_prompt="Click point on UI"
-        user_pose="{goal}"
-        view_name="Visualization"
-      />
-      <Action
-        ID="ComputePathToPoseAction"
-        action_name="/compute_path_to_pose"
-        goal="{goal}"
-        ignore_stamp_time="true"
-        path="{path}"
-        planning_time="{planning_time}"
-        start="{start}"
-        timeout_sec="-1.000000"
-        use_start="false"
-        planner_id="GridBased"
-      />
-      <Action ID="WaitForUserPathApproval" path="{path}" />
-      <Action
-        ID="FollowPathAction"
-        action_name="/follow_path"
-        controller_id="FollowPath"
-        distance_to_goal="{distance_to_goal}"
-        goal_checker_id="general_goal_checker"
-        path="{path}"
-        speed="{speed}"
-        timeout_sec="-1.000000"
-      />
+      <Action ID="CreateStampedPose" reference_frame="base_link" stamped_pose="{start}"/>
+      <Action ID="ActivateControllers" controller_names="platform_velocity_controller_nav2"/>
+      <Action ID="GetPoseFromUser" is_normal="true" pose_prompt="Click point on UI" user_pose="{goal}" view_name="Visualization"/>
+      <Action ID="ComputePathToPoseAction" action_name="/compute_path_to_pose" goal="{goal}" ignore_stamp_time="true" path="{path}" planning_time="{planning_time}" start="{start}" timeout_sec="-1.000000" use_start="false" planner_id="GridBased"/>
+      <Action ID="WaitForUserPathApproval" path="{path}"/>
+      <Action ID="FollowPathAction" action_name="/follow_path" controller_id="FollowPath" distance_to_goal="{distance_to_goal}" goal_checker_id="general_goal_checker" path="{path}" speed="{speed}" timeout_sec="-1.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Navigate to Clicked Point">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Navigation" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Navigation"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/open_gripper.xml b/src/hangar_sim/objectives/open_gripper.xml
index d5e32b63..e72d92d7 100644
--- a/src/hangar_sim/objectives/open_gripper.xml
+++ b/src/hangar_sim/objectives/open_gripper.xml
@@ -1,21 +1,16 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Open Gripper">
-  <BehaviorTree
-    ID="Open Gripper"
-    _description="Open the gripper"
-    _favorite="true"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Open Gripper" _description="Open the gripper" _favorite="true" _subtreeOnly="true">
     <Control ID="Sequence" name="root">
       <!--Not used in this config-->
-      <Action ID="AlwaysFailure" />
+      <Action ID="AlwaysFailure"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Open Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
-        <Metadata runnable="false" />
+        <Metadata subcategory="Grasping"/>
+        <Metadata runnable="false"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/request_teleoperation.xml b/src/hangar_sim/objectives/request_teleoperation.xml
index b0768102..1f1a95e6 100644
--- a/src/hangar_sim/objectives/request_teleoperation.xml
+++ b/src/hangar_sim/objectives/request_teleoperation.xml
@@ -1,65 +1,32 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Request Teleoperation">
   <!--The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message.-->
-  <BehaviorTree
-    ID="Request Teleoperation"
-    _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective."
-    _favorite="false"
-    _hardcoded="false"
-    _subtreeOnly="true"
-    enable_user_interaction="false"
-    initial_teleop_mode="3"
-  >
+  <BehaviorTree ID="Request Teleoperation" _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective." _favorite="false" _hardcoded="false" _subtreeOnly="true" enable_user_interaction="false" initial_teleop_mode="3">
     <Control ID="Sequence">
-      <Action ID="Script" code="teleop_mode := 0" />
+      <Action ID="Script" code="teleop_mode := 0"/>
       <Control ID="Parallel" success_count="1" failure_count="1">
-        <Action
-          ID="DoTeleoperateAction"
-          enable_user_interaction="{enable_user_interaction}"
-          user_interaction_prompt="{user_interaction_prompt}"
-          initial_teleop_mode="{initial_teleop_mode}"
-          current_teleop_mode="{teleop_mode}"
-        />
+        <Action ID="DoTeleoperateAction" enable_user_interaction="{enable_user_interaction}" user_interaction_prompt="{user_interaction_prompt}" initial_teleop_mode="{initial_teleop_mode}" current_teleop_mode="{teleop_mode}"/>
         <Decorator ID="KeepRunningUntilFailure">
           <Control ID="Sequence">
             <!--Closing and opening the gripper-->
             <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 7">
-              <SubTree ID="Close Gripper" />
+              <SubTree ID="Close Gripper"/>
             </Decorator>
             <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 6">
-              <SubTree ID="Open Gripper" />
+              <SubTree ID="Open Gripper"/>
             </Decorator>
             <!--Joint sliders interpolate to joint state-->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 5">
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrieveJointStateParameter"
-                      timeout_sec="2"
-                      joint_state="{target_joint_state}"
-                    />
-                    <SubTree
-                      ID="Interpolate to Joint State"
-                      _collapsed="false"
-                      target_joint_state="{target_joint_state}"
-                      controller_names="/joint_trajectory_controller"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrieveJointStateParameter" timeout_sec="2" joint_state="{target_joint_state}"/>
+                    <SubTree ID="Interpolate to Joint State" _collapsed="false" target_joint_state="{target_joint_state}" controller_names="/joint_trajectory_controller"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
@@ -69,31 +36,13 @@
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrievePoseParameter"
-                      timeout_sec="-1"
-                      pose="{target_pose}"
-                    />
-                    <SubTree
-                      ID="Move to Pose"
-                      _collapsed="false"
-                      target_pose="{target_pose}"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrievePoseParameter" timeout_sec="-1" pose="{target_pose}"/>
+                    <SubTree ID="Move to Pose" _collapsed="false" target_pose="{target_pose}"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
@@ -103,56 +52,23 @@
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrieveJointStateParameter"
-                      timeout_sec="-1"
-                      joint_state="{target_joint_state}"
-                    />
-                    <SubTree
-                      ID="Move to Joint State"
-                      _collapsed="false"
-                      target_joint_state="{target_joint_state}"
-                      acceleration_scale_factor="1.0"
-                      controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-                      controller_names="joint_trajectory_controller"
-                      joint_group_name="manipulator"
-                      keep_orientation="false"
-                      keep_orientation_link_names="grasp_link"
-                      keep_orientation_tolerance="0.05"
-                      link_padding="0.0"
-                      velocity_scale_factor="1.0"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
+                    <SubTree ID="Move to Joint State" _collapsed="false" target_joint_state="{target_joint_state}" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.05" link_padding="0.0" velocity_scale_factor="1.0"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
             </Decorator>
             <!--Cartesian and joint jogging-->
             <Control ID="Sequence" _while="teleop_mode == 2">
-              <Action
-                ID="TeleoperateTwist"
-                controller_name="servo_controller"
-              />
+              <Action ID="TeleoperateTwist" controller_name="servo_controller"/>
             </Control>
             <Control ID="Sequence" _while="teleop_mode == 1">
-              <Action
-                ID="TeleoperateJointJog"
-                controller_name="servo_controller"
-              />
+              <Action ID="TeleoperateJointJog" controller_name="servo_controller"/>
             </Control>
           </Control>
         </Decorator>
@@ -161,12 +77,12 @@
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Request Teleoperation">
-      <input_port name="enable_user_interaction" default="false" />
-      <input_port name="initial_teleop_mode" default="3" />
-      <input_port name="user_interaction_prompt" default="" />
+      <input_port name="enable_user_interaction" default="false"/>
+      <input_port name="initial_teleop_mode" default="3"/>
+      <input_port name="user_interaction_prompt" default=""/>
       <MetadataFields>
-        <Metadata subcategory="User Input" />
-        <Metadata runnable="false" />
+        <Metadata subcategory="User Input"/>
+        <Metadata runnable="false"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/segment_image_from_point.xml b/src/hangar_sim/objectives/segment_image_from_point.xml
index 81e1dd60..ac59e636 100644
--- a/src/hangar_sim/objectives/segment_image_from_point.xml
+++ b/src/hangar_sim/objectives/segment_image_from_point.xml
@@ -1,30 +1,16 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Segment Image from Point">
   <!--//////////-->
-  <BehaviorTree
-    ID="Segment Image from Point"
-    _description="Captures an image and segments it using the user provided prompt points."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Segment Image from Point" _description="Captures an image and segments it using the user provided prompt points." _favorite="true">
     <Control ID="Sequence">
-      <SubTree
-        ID="Segment Image from Point Subtree"
-        _collapsed="true"
-        decoder_model_path="models/decoder.onnx"
-        encoder_model_path="models/sam2_hiera_large_encoder.onnx"
-        image_topic_name="/wrist_camera/color"
-        masks_visualization_topic="/masks_visualization"
-        model_package="lab_sim"
-        pixel_coords="{pixel_coords}"
-        masks2d="{masks2d}"
-      />
+      <SubTree ID="Segment Image from Point Subtree" _collapsed="true" decoder_model_path="models/decoder.onnx" encoder_model_path="models/sam2_hiera_large_encoder.onnx" image_topic_name="/wrist_camera/color" masks_visualization_topic="/masks_visualization" model_package="lab_sim" pixel_coords="{pixel_coords}" masks2d="{masks2d}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Segment Image from Point">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Perception - ML" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Perception - ML"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/segment_image_from_text_prompt.xml b/src/hangar_sim/objectives/segment_image_from_text_prompt.xml
index aee28e10..715c058f 100644
--- a/src/hangar_sim/objectives/segment_image_from_text_prompt.xml
+++ b/src/hangar_sim/objectives/segment_image_from_text_prompt.xml
@@ -1,33 +1,16 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Segment Image from Text Prompt">
   <!--//////////-->
-  <BehaviorTree
-    ID="Segment Image from Text Prompt"
-    _subtreeOnly="false"
-    _description="Run text based segmentation and visualize he masks."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Segment Image from Text Prompt" _subtreeOnly="false" _description="Run text based segmentation and visualize he masks." _favorite="true">
     <Control ID="Sequence">
-      <SubTree
-        ID="Segment Image from Text Prompt Subtree"
-        _collapsed="true"
-        clip_model_path="models/clip.onnx"
-        clipseg_model_path="models/clipseg.onnx"
-        erosion_size="0"
-        masks_visualization_topic="/masks_visualization"
-        model_package="lab_sim"
-        threshold="0.5"
-        image_topic_name="/wrist_camera/color"
-        prompts="small boxes"
-        negative_prompts="grey"
-      />
+      <SubTree ID="Segment Image from Text Prompt Subtree" _collapsed="true" clip_model_path="models/clip.onnx" clipseg_model_path="models/clipseg.onnx" erosion_size="0" masks_visualization_topic="/masks_visualization" model_package="lab_sim" threshold="0.5" image_topic_name="/wrist_camera/color" prompts="small boxes" negative_prompts="grey"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Segment Image from Text Prompt">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Perception - ML" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Perception - ML"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/segment_point_cloud_from_clicked_point.xml b/src/hangar_sim/objectives/segment_point_cloud_from_clicked_point.xml
index 783f0ce9..aeb267a8 100644
--- a/src/hangar_sim/objectives/segment_point_cloud_from_clicked_point.xml
+++ b/src/hangar_sim/objectives/segment_point_cloud_from_clicked_point.xml
@@ -1,33 +1,16 @@
-<?xml version="1.0" encoding="utf-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Segment Point Cloud from Clicked Point"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Segment Point Cloud from Clicked Point">
   <!--//////////-->
-  <BehaviorTree
-    ID="Segment Point Cloud from Clicked Point"
-    _description="Captures a point cloud and requests the user to click an object in the image to be segmented. The point cloud is then filtered to only include the selected object."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Segment Point Cloud from Clicked Point" _description="Captures a point cloud and requests the user to click an object in the image to be segmented. The point cloud is then filtered to only include the selected object." _favorite="true">
     <Control ID="Sequence">
-      <SubTree
-        ID="Segment Point Cloud from Clicked Point Subtree"
-        _collapsed="true"
-        camera_topic_name="/wrist_camera/camera_info"
-        decoder_model_path="models/decoder.onnx"
-        encoder_model_path="models/sam2_hiera_large_encoder.onnx"
-        image_topic_name="/wrist_camera/color"
-        masks_visualization_topic="/masks_visualization"
-        model_package="lab_sim"
-        points_topic_name="/wrist_camera/points"
-      />
+      <SubTree ID="Segment Point Cloud from Clicked Point Subtree" _collapsed="true" camera_topic_name="/wrist_camera/camera_info" decoder_model_path="models/decoder.onnx" encoder_model_path="models/sam2_hiera_large_encoder.onnx" image_topic_name="/wrist_camera/color" masks_visualization_topic="/masks_visualization" model_package="lab_sim" points_topic_name="/wrist_camera/points"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Segment Point Cloud from Clicked Point">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Perception - ML" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Perception - ML"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/take_snap.xml b/src/hangar_sim/objectives/take_snap.xml
index bb2d2ff1..eda52901 100644
--- a/src/hangar_sim/objectives/take_snap.xml
+++ b/src/hangar_sim/objectives/take_snap.xml
@@ -1,21 +1,18 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Take scene camera snapshot">
   <!--//////////-->
-  <BehaviorTree
-    ID="Take scene camera snapshot"
-    _description="Capture a point cloud snapshot of the scene camera and send it to the UI"
-    _favorite="false"
-  >
+  <BehaviorTree ID="Take scene camera snapshot" _description="Capture a point cloud snapshot of the scene camera and send it to the UI" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <!--Clear out old snapshot data-->
-      <Action ID="ClearSnapshot" />
-      <Action ID="GetPointCloud" topic_name="/scene_camera/points" />
-      <Action ID="SendPointCloudToUI" />
+      <Action ID="ClearSnapshot"/>
+      <Action ID="GetPointCloud" topic_name="/scene_camera/points"/>
+      <Action ID="SendPointCloudToUI"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Take scene camera snapshot">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/objectives/wrist_snap.xml b/src/hangar_sim/objectives/wrist_snap.xml
index 55a7052d..6fad5761 100644
--- a/src/hangar_sim/objectives/wrist_snap.xml
+++ b/src/hangar_sim/objectives/wrist_snap.xml
@@ -1,20 +1,17 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Take wrist camera snapshot">
   <!--//////////-->
-  <BehaviorTree
-    ID="Take wrist camera snapshot"
-    _description="Capture a point cloud snapshot of the wrist camera and send it to the UI"
-    _favorite="false"
-  >
+  <BehaviorTree ID="Take wrist camera snapshot" _description="Capture a point cloud snapshot of the wrist camera and send it to the UI" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Do not clear snapshots first, so we can take multiple snapshots from different angles -->
-      <Action ID="GetPointCloud" topic_name="/wrist_camera/points" />
-      <Action ID="SendPointCloudToUI" />
+      <Action ID="GetPointCloud" topic_name="/wrist_camera/points"/>
+      <Action ID="SendPointCloudToUI"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Take wrist camera snapshot">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/hangar_sim/package.xml b/src/hangar_sim/package.xml
index 6119118f..725dbdc3 100644
--- a/src/hangar_sim/package.xml
+++ b/src/hangar_sim/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>hangar_sim</name>
   <version>6.5.0</version>
diff --git a/src/lab_sim/description/scene.xml b/src/lab_sim/description/scene.xml
index 9e7726e7..f8cc0e42 100644
--- a/src/lab_sim/description/scene.xml
+++ b/src/lab_sim/description/scene.xml
@@ -1,81 +1,46 @@
+<?xml version='1.0' encoding='utf-8'?>
 <!-- UR5e inspired from https://github.com/google-deepmind/mujoco_menagerie/tree/main/universal_robots_ur5e -->
 <mujoco model="ur5e scene">
-  <include file="ur5e/ur5e_linear_rail_globals.xml" />
-  <include file="lab_desk/desk_globals.xml" />
-  <include file="robotiq_2f85/robotiq2f85_globals.xml" />
+  <include file="ur5e/ur5e_linear_rail_globals.xml"/>
+  <include file="lab_desk/desk_globals.xml"/>
+  <include file="robotiq_2f85/robotiq2f85_globals.xml"/>
 
-  <compiler angle="radian" autolimits="true" />
+  <compiler angle="radian" autolimits="true"/>
 
-  <option integrator="implicitfast" />
+  <option integrator="implicitfast"/>
 
   <asset>
-    <mesh name="cube" file="assets/Cube.stl" />
+    <mesh name="cube" file="assets/Cube.stl"/>
   </asset>
 
   <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"/>
 
-    <light
-      name="spotlight"
-      mode="targetbodycom"
-      target="wrist_2_link"
-      pos="0 -1 2"
-    />
-    <geom
-      name="floor"
-      pos="0 0 -0.512"
-      size="0 0 0.05"
-      type="plane"
-      material="groundplane"
-    />
+    <light name="spotlight" mode="targetbodycom" target="wrist_2_link" pos="0 -1 2"/>
+    <geom name="floor" pos="0 0 -0.512" size="0 0 0.05" type="plane" material="groundplane"/>
     <!-- Scene Camera -->
-    <site
-      name="scene_camera_optical_frame"
-      pos="-1.0 -2.0 2.966"
-      euler="3.99 0.000 0"
-    />
-    <camera
-      name="scene_camera"
-      pos="-1.0 -2.0 2.966"
-      fovy="58"
-      mode="fixed"
-      resolution="640 480"
-      euler="0.849 0.000 0.0"
-    />
+    <site name="scene_camera_optical_frame" pos="-1.0 -2.0 2.966" euler="3.99 0.000 0"/>
+    <camera name="scene_camera" pos="-1.0 -2.0 2.966" fovy="58" mode="fixed" resolution="640 480" euler="0.849 0.000 0.0"/>
     <!-- lab desk -->
-    <include file="lab_desk/desk.xml" />
+    <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" />
-      <geom
-        type="box"
-        size="0.025 0.025 0.025"
-        rgba="1 1 1 1"
-        material="apriltag_material1"
-      />
+      <freejoint name="block1"/>
+      <geom type="box" size="0.025 0.025 0.025" rgba="1 1 1 1" material="apriltag_material1"/>
     </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" />
+      <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"/>
     </body>
     <body name="block3" pos="-0.2 0.75 0.575">
-      <freejoint name="block3" />
-      <geom
-        type="box"
-        size="0.025 0.025 0.025"
-        rgba="1 1 1 1"
-        material="apriltag_material2"
-      />
+      <freejoint name="block3"/>
+      <geom type="box" size="0.025 0.025 0.025" rgba="1 1 1 1" material="apriltag_material2"/>
     </body>
-    <include file="ur5e/ur5e_linear_rail.xml" />
+    <include file="ur5e/ur5e_linear_rail.xml"/>
   </worldbody>
 
   <keyframe>
-    <key
-      qpos="-0.886764 0.727551 0.472375 -0.834559 -6.95805e-05 -6.1375e-05 -0.550919 -0.499647 0.689928 0.470942 -0.749838 3.91843e-05 -5.08119e-05 -0.661622 -0.3609 0.609537 0.470941 -0.757741 2.96011e-05 4.00136e-05 -0.652556 -0.303591 0.696467 0.470989 -0.7788 -5.32927e-05 2.99347e-06 -0.627272 -0.000373058 0.88828 0.473534 -0.838137 -1.0566e-05 -3.07868e-06 0.54546 -0.0963647 0.840186 0.473528 0.281469 -1.36042e-05 6.60035e-06 -0.95957 0.0484372 0.842182 0.473688 -0.758342 6.07647e-06 2.73022e-05 -0.651856 0.2 0.75 0.499376 1 0 0 0 1.70629e-05 0.749992 0.499377 1 0.00016562 0.000360588 1.95345e-10 -0.2 0.75 0.499376 1 0 0 0 0 0 -1.88491 1.10028 -1.25653 -1.57075 -1.08677e-12 0.0208231 0.000536851 0.0221006 -0.022866 0.020837 0.000547055 0.0221413 -0.0229382"
-      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"
-      ctrl="0 -1.8849 1.0997 -1.2566 -1.57075 0 0 0"
-    />
+    <key qpos="-0.886764 0.727551 0.472375 -0.834559 -6.95805e-05 -6.1375e-05 -0.550919 -0.499647 0.689928 0.470942 -0.749838 3.91843e-05 -5.08119e-05 -0.661622 -0.3609 0.609537 0.470941 -0.757741 2.96011e-05 4.00136e-05 -0.652556 -0.303591 0.696467 0.470989 -0.7788 -5.32927e-05 2.99347e-06 -0.627272 -0.000373058 0.88828 0.473534 -0.838137 -1.0566e-05 -3.07868e-06 0.54546 -0.0963647 0.840186 0.473528 0.281469 -1.36042e-05 6.60035e-06 -0.95957 0.0484372 0.842182 0.473688 -0.758342 6.07647e-06 2.73022e-05 -0.651856 0.2 0.75 0.499376 1 0 0 0 1.70629e-05 0.749992 0.499377 1 0.00016562 0.000360588 1.95345e-10 -0.2 0.75 0.499376 1 0 0 0 0 0 -1.88491 1.10028 -1.25653 -1.57075 -1.08677e-12 0.0208231 0.000536851 0.0221006 -0.022866 0.020837 0.000547055 0.0221413 -0.0229382" 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" ctrl="0 -1.8849 1.0997 -1.2566 -1.57075 0 0 0"/>
   </keyframe>
 </mujoco>
diff --git a/src/lab_sim/launch/agent_bridge.launch.xml b/src/lab_sim/launch/agent_bridge.launch.xml
index b2aa7543..8eeec7d9 100644
--- a/src/lab_sim/launch/agent_bridge.launch.xml
+++ b/src/lab_sim/launch/agent_bridge.launch.xml
@@ -1,6 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
 </launch>
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 47279c01..60b623c4 100644
--- a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
+++ b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
@@ -1,100 +1,31 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="3 Waypoints Pick and Place">
   <!--//////////-->
-  <BehaviorTree
-    ID="3 Waypoints Pick and Place"
-    _description="Basic example of repeatedly grabbing a small (invisible) object, placing it at desired destination, and then moving back to a home position"
-    _favorite="true"
-  >
+  <BehaviorTree ID="3 Waypoints Pick and Place" _description="Basic example of repeatedly grabbing a small (invisible) object, placing it at desired destination, and then moving back to a home position" _favorite="true">
     <Control ID="Sequence">
       <!--Setup the environment to its "home" configuration - move to center and open gripper-->
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Look at Table"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller"
-      />
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0"
-      />
+      <SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
       <!--Run pick and place on loop-->
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
           <!--Pick object from original location, put it down at a different part of the table, and go back to center pose-->
           <Control ID="Sequence">
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Pick Cube"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller"
-            />
-            <Action
-              ID="MoveGripperAction"
-              gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-              position="0.7929"
-            />
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Look at Table"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller"
-            />
-            <SubTree
-              ID="Move to Waypoint"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller"
-              waypoint_name="Place Cube"
-            />
-            <Action
-              ID="MoveGripperAction"
-              gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-              position="0"
-            />
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Look at Table"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller"
-            />
+            <SubTree ID="Move to Waypoint" waypoint_name="Pick Cube" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
+            <SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
+            <SubTree ID="Move to Waypoint" joint_group_name="manipulator" controller_names="/joint_trajectory_controller" waypoint_name="Place Cube"/>
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
+            <SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
           </Control>
           <!--Pick object from where it was placed, put it down on the original location, and go back to center pose-->
           <Control ID="Sequence">
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Place Cube"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller"
-            />
-            <Action
-              ID="MoveGripperAction"
-              gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-              position="0.7929"
-            />
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Look at Table"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller"
-            />
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Pick Cube"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller"
-            />
-            <Action
-              ID="MoveGripperAction"
-              gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-              position="0"
-            />
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Look at Table"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller"
-            />
+            <SubTree ID="Move to Waypoint" waypoint_name="Place Cube" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
+            <SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
+            <SubTree ID="Move to Waypoint" waypoint_name="Pick Cube" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
+            <SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
           </Control>
         </Control>
       </Decorator>
@@ -103,7 +34,7 @@
   <TreeNodesModel>
     <SubTree ID="3 Waypoints Pick and Place">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/_scan_scene.xml b/src/lab_sim/objectives/_scan_scene.xml
index 358415fb..b65739a1 100644
--- a/src/lab_sim/objectives/_scan_scene.xml
+++ b/src/lab_sim/objectives/_scan_scene.xml
@@ -1,37 +1,19 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="_Scan Scene">
   <!--//////////-->
-  <BehaviorTree
-    ID="_Scan Scene"
-    _description="Move robot out of scene to enable full snapshot of scene."
-    _favorite="true"
-  >
+  <BehaviorTree ID="_Scan Scene" _description="Move robot out of scene to enable full snapshot of scene." _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="ClearSnapshot" />
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        joint_group_name="manipulator"
-        planner_interface="moveit_default"
-        controller_names="/joint_trajectory_controller"
-        waypoint_name="Park Far Right"
-      />
-      <SubTree ID="Take scene camera snapshot" _collapsed="true" />
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        joint_group_name="manipulator"
-        planner_interface="moveit_default"
-        controller_names="/joint_trajectory_controller"
-        waypoint_name="Look at Table"
-      />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Move to Waypoint" _collapsed="true" joint_group_name="manipulator" planner_interface="moveit_default" controller_names="/joint_trajectory_controller" waypoint_name="Park Far Right"/>
+      <SubTree ID="Take scene camera snapshot" _collapsed="true"/>
+      <SubTree ID="Move to Waypoint" _collapsed="true" joint_group_name="manipulator" planner_interface="moveit_default" controller_names="/joint_trajectory_controller" waypoint_name="Look at Table"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="_Scan Scene">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
index 39b45bac..9dd04fb4 100644
--- a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
+++ b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
@@ -1,49 +1,21 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Add Waypoints to MTC Task">
   <!--//////////-->
-  <BehaviorTree
-    ID="Add Waypoints to MTC Task"
-    _subtreeOnly="true"
-    _description="Retrieves waypoints and adds them to an MTC task for motion planning."
-    joint_group_name="manipulator"
-    mtc_task="{mtc_task}"
-  >
+  <BehaviorTree ID="Add Waypoints to MTC Task" _subtreeOnly="true" _description="Retrieves waypoints and adds them to an MTC task for motion planning." joint_group_name="manipulator" mtc_task="{mtc_task}">
     <Control ID="Sequence">
-      <Action
-        ID="RetrieveWaypoint"
-        waypoint_joint_state="{target_joint_state}"
-        waypoint_name="Look at Table"
-        joint_group_name="{joint_group_name}"
-      />
-      <Action
-        ID="SetupMTCPlanToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCPlanToJointState"
-        planning_group_name="{joint_group_name}"
-        task="{mtc_task}"
-      />
-      <Action
-        ID="RetrieveWaypoint"
-        waypoint_joint_state="{target_joint_state}"
-        waypoint_name="Workspace Right"
-        joint_group_name="{joint_group_name}"
-      />
-      <Action
-        ID="SetupMTCPlanToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCPlanToJointState"
-        planning_group_name="{joint_group_name}"
-        task="{mtc_task}"
-      />
+      <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="Look at Table" joint_group_name="{joint_group_name}"/>
+      <Action ID="SetupMTCPlanToJointState" joint_state="{target_joint_state}" name="SetupMTCPlanToJointState" planning_group_name="{joint_group_name}" task="{mtc_task}"/>
+      <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="Workspace Right" joint_group_name="{joint_group_name}"/>
+      <Action ID="SetupMTCPlanToJointState" joint_state="{target_joint_state}" name="SetupMTCPlanToJointState" planning_group_name="{joint_group_name}" task="{mtc_task}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Add Waypoints to MTC Task">
-      <input_port name="joint_group_name" default="manipulator" />
-      <inout_port name="mtc_task" default="{mtc_task}" />
+      <input_port name="joint_group_name" default="manipulator"/>
+      <inout_port name="mtc_task" default="{mtc_task}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Task Planning" />
-        <Metadata runnable="false" />
+        <Metadata subcategory="Motion - Task Planning"/>
+        <Metadata runnable="false"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/apriltag_pick_object.xml b/src/lab_sim/objectives/apriltag_pick_object.xml
index 22d9dac7..6799c448 100644
--- a/src/lab_sim/objectives/apriltag_pick_object.xml
+++ b/src/lab_sim/objectives/apriltag_pick_object.xml
@@ -1,115 +1,36 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
   <!--//////////-->
-  <BehaviorTree
-    ID="Pick April Tag Labeled Object"
-    _description="Picks up an object that has an AprilTag marker."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Pick April Tag Labeled Object" _description="Picks up an object that has an AprilTag marker." _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Look at table" _collapsed="true" />
-      <Action ID="ClearSnapshot" />
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Look at table" _collapsed="true"/>
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
       <Control ID="Sequence" name="Setup">
-        <SubTree ID="Open Gripper" />
+        <SubTree ID="Open Gripper"/>
       </Control>
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="apriltag_detection_config.yaml"
-        parameters="{parameters}"
-      />
-      <Action
-        ID="GetCameraInfo"
-        topic_name="/wrist_camera/camera_info"
-        message_out="{camera_info}"
-        timeout_sec="5.000000"
-      />
-      <Action
-        ID="GetImage"
-        topic_name="/wrist_camera/color"
-        message_out="{image}"
-        timeout_sec="5.000000"
-      />
-      <SubTree
-        ID="Get AprilTag Pose from Image"
-        image="{image}"
-        camera_info="{camera_info}"
-        parameters="{parameters}"
-        _collapsed="true"
-        detection_pose="{detection_pose}"
-      />
-      <Action
-        ID="TransformPose"
-        output_pose="{view_pose}"
-        input_pose="{detection_pose}"
-        quaternion_xyzw="0;0;0;1"
-        translation_xyz="0;-0.1;-0.15"
-      />
-      <SubTree
-        ID="Plan Move To Pose"
-        target_pose="{view_pose}"
-        move_to_pose_solution="{move_to_pose_solution}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{move_to_pose_solution}"
-        goal_duration_tolerance="-1.000000"
-      />
-      <SubTree
-        ID="Get AprilTag Pose from Image"
-        image="{image}"
-        camera_info="{camera_info}"
-        parameters="{parameters}"
-        _collapsed="true"
-        detection_pose="{detection_pose}"
-      />
-      <Action
-        ID="TransformPose"
-        output_pose="{offset_pose}"
-        input_pose="{detection_pose}"
-        quaternion_xyzw="0;0;0;1"
-        translation_xyz="0;0;-0.01"
-      />
-      <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}"
-        skip_collision_check="false"
-      />
-      <Action
-        ID="SetupMTCPickObject"
-        grasp_pose="{offset_pose}"
-        task="{pick_object_task}"
-        parameters="{parameters}"
-        monitored_stage="current state"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{pick_object_solution}"
-        task="{pick_object_task}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{pick_object_solution}"
-        goal_duration_tolerance="-1.000000"
-      />
+      <Action ID="LoadObjectiveParameters" config_file_name="apriltag_detection_config.yaml" parameters="{parameters}"/>
+      <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" message_out="{camera_info}" timeout_sec="5.000000"/>
+      <Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}" timeout_sec="5.000000"/>
+      <SubTree ID="Get AprilTag Pose from Image" image="{image}" camera_info="{camera_info}" parameters="{parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
+      <Action ID="TransformPose" output_pose="{view_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;-0.1;-0.15"/>
+      <SubTree ID="Plan Move To Pose" target_pose="{view_pose}" move_to_pose_solution="{move_to_pose_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" goal_duration_tolerance="-1.000000"/>
+      <SubTree ID="Get AprilTag Pose from Image" image="{image}" camera_info="{camera_info}" parameters="{parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
+      <Action ID="TransformPose" output_pose="{offset_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;-0.01"/>
+      <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}" skip_collision_check="false"/>
+      <Action ID="SetupMTCPickObject" grasp_pose="{offset_pose}" task="{pick_object_task}" parameters="{parameters}" monitored_stage="current state"/>
+      <Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
+      <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" goal_duration_tolerance="-1.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Pick April Tag Labeled Object">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/clear_previous_obstacles.xml b/src/lab_sim/objectives/clear_previous_obstacles.xml
index dcf5dd0f..e72aabb3 100644
--- a/src/lab_sim/objectives/clear_previous_obstacles.xml
+++ b/src/lab_sim/objectives/clear_previous_obstacles.xml
@@ -1,19 +1,16 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Clear Previous Obstacles">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Clear Previous Obstacles"
-    _subtreeOnly="true"
-    _description="Clear previous scene obstacles"
-  >
+  <BehaviorTree ID="Clear Previous Obstacles" _subtreeOnly="true" _description="Clear previous scene obstacles">
     <Control ID="Sequence" name="Setup">
-      <Action ID="ClearSnapshot" />
-      <SubTree ID="Reset Planning Scene" _collapsed="true" />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Reset Planning Scene" _collapsed="true"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Clear Previous Obstacles">
       <MetadataFields>
-        <Metadata subcategory="Perception - Scene Constraints" />
+        <Metadata subcategory="Perception - Scene Constraints"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/close_gripper.xml b/src/lab_sim/objectives/close_gripper.xml
index ec502a1b..5c3d673f 100644
--- a/src/lab_sim/objectives/close_gripper.xml
+++ b/src/lab_sim/objectives/close_gripper.xml
@@ -1,28 +1,18 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Close Gripper">
-  <BehaviorTree
-    ID="Close Gripper"
-    _description="Close the gripper"
-    _favorite="true"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Close Gripper" _description="Close the gripper" _favorite="true" _subtreeOnly="false">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.7929"
-        timeout="15.000000"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929" timeout="15.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Close Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Grasping"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
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 72014475..4ae088b9 100644
--- a/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml
+++ b/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml
@@ -1,121 +1,32 @@
-<?xml version="1.0" encoding="utf-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Constrained Pick and Place Subtree"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Constrained Pick and Place Subtree">
   <!--//////////-->
-  <BehaviorTree
-    ID="Constrained Pick and Place Subtree"
-    _subtreeOnly="true"
-    _description="Takes in waypoints to run a constrained pick and place operation."
-  >
+  <BehaviorTree ID="Constrained Pick and Place Subtree" _subtreeOnly="true" _description="Takes in waypoints to run a constrained pick and place operation.">
     <Control ID="Sequence">
       <!--Move to pick location-->
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="{pre_pick}"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="{pick}"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        keep_orientation_link_names="grasp_link"
-      />
+      <SubTree ID="Move to Waypoint" waypoint_name="{pre_pick}" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" keep_orientation="true" keep_orientation_tolerance="0.2" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" link_padding="0.01" velocity_scale_factor="1.0" keep_orientation_link_names="grasp_link"/>
+      <SubTree ID="Move to Waypoint" waypoint_name="{pick}" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" keep_orientation="true" keep_orientation_tolerance="0.2" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" link_padding="0.01" velocity_scale_factor="1.0" 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" />
+        <SubTree ID="Close Gripper"/>
       </Decorator>
       <!--Move to place (drop) location-->
-      <SubTree
-        ID="Move to Waypoint"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        joint_group_name="manipulator"
-        planner_interface="moveit_default"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        _collapsed="true"
-        waypoint_name="{pre_pick}"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree
-        ID="Move to Waypoint"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        joint_group_name="manipulator"
-        planner_interface="moveit_default"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        _collapsed="true"
-        waypoint_name="{pre_place}"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree
-        ID="Move to Waypoint"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        joint_group_name="manipulator"
-        planner_interface="moveit_default"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        _collapsed="true"
-        waypoint_name="{place}"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree ID="Open Gripper" />
-      <SubTree
-        ID="Move to Waypoint"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        joint_group_name="manipulator"
-        planner_interface="moveit_default"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        _collapsed="true"
-        waypoint_name="{pre_place}"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        keep_orientation_link_names="grasp_link"
-      />
+      <SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" keep_orientation="true" keep_orientation_tolerance="0.2" _collapsed="true" waypoint_name="{pre_pick}" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" link_padding="0.01" velocity_scale_factor="1.0" keep_orientation_link_names="grasp_link"/>
+      <SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" keep_orientation="true" keep_orientation_tolerance="0.2" _collapsed="true" waypoint_name="{pre_place}" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" link_padding="0.01" velocity_scale_factor="1.0" keep_orientation_link_names="grasp_link"/>
+      <SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" keep_orientation="true" keep_orientation_tolerance="0.2" _collapsed="true" waypoint_name="{place}" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" link_padding="0.01" velocity_scale_factor="1.0" keep_orientation_link_names="grasp_link"/>
+      <SubTree ID="Open Gripper"/>
+      <SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" keep_orientation="true" keep_orientation_tolerance="0.2" _collapsed="true" waypoint_name="{pre_place}" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" link_padding="0.01" velocity_scale_factor="1.0" keep_orientation_link_names="grasp_link"/>
     </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="" />
+      <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" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/constrained_pick_place.xml b/src/lab_sim/objectives/constrained_pick_place.xml
index fecc6de7..cfad2716 100644
--- a/src/lab_sim/objectives/constrained_pick_place.xml
+++ b/src/lab_sim/objectives/constrained_pick_place.xml
@@ -1,58 +1,19 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move Beakers to Burners">
   <!--//////////-->
-  <BehaviorTree
-    ID="Move Beakers to Burners"
-    _description="Example of picking and placing objects with orientation constraints enforced on the arm's motion"
-    _favorite="true"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Move Beakers to Burners" _description="Example of picking and placing objects with orientation constraints enforced on the arm's motion" _favorite="true" _subtreeOnly="false">
     <Control ID="Sequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        waypoint_name="Beaker 1a - Pre Pick"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller"
-        velocity_scale_factor="1.0"
-        acceleration_scale_factor="1.0"
-        link_padding="0.01"
-        keep_orientation="false"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        keep_orientation_tolerance="0.05"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree ID="Open Gripper" />
-      <SubTree
-        ID="Constrained Pick and Place Subtree"
-        pre_pick="Beaker 1a - Pre Pick"
-        pick="Beaker 1b - Pick"
-        pre_place="Beaker 1c - Pre Place"
-        place="Beaker 1d - Place"
-        name="Beaker 1"
-      />
-      <SubTree
-        ID="Constrained Pick and Place Subtree"
-        pre_pick="Beaker 2a - Pre Pick"
-        pick="Beaker 2b - Pick"
-        pre_place="Beaker 2c - Pre Place"
-        place="Beaker 2d - Place"
-        name="Beaker 2"
-      />
-      <SubTree
-        ID="Constrained Pick and Place Subtree"
-        pre_pick="Beaker 3a - Pre Pick"
-        pick="Beaker 3b - Pick"
-        pre_place="Beaker 3c - Pre Place"
-        place="Beaker 3d - Place"
-        name="Beaker 3"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Beaker 1a - Pre Pick" joint_group_name="manipulator" controller_names="/joint_trajectory_controller" velocity_scale_factor="1.0" acceleration_scale_factor="1.0" link_padding="0.01" keep_orientation="false" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" keep_orientation_tolerance="0.05" keep_orientation_link_names="grasp_link"/>
+      <SubTree ID="Open Gripper"/>
+      <SubTree ID="Constrained Pick and Place Subtree" pre_pick="Beaker 1a - Pre Pick" pick="Beaker 1b - Pick" pre_place="Beaker 1c - Pre Place" place="Beaker 1d - Place" name="Beaker 1"/>
+      <SubTree ID="Constrained Pick and Place Subtree" pre_pick="Beaker 2a - Pre Pick" pick="Beaker 2b - Pick" pre_place="Beaker 2c - Pre Place" place="Beaker 2d - Place" name="Beaker 2"/>
+      <SubTree ID="Constrained Pick and Place Subtree" pre_pick="Beaker 3a - Pre Pick" pick="Beaker 3b - Pick" pre_place="Beaker 3c - Pre Place" place="Beaker 3d - Place" name="Beaker 3"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move Beakers to Burners">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/create_pose_vector.xml b/src/lab_sim/objectives/create_pose_vector.xml
index 49c21e1f..5bf498f3 100644
--- a/src/lab_sim/objectives/create_pose_vector.xml
+++ b/src/lab_sim/objectives/create_pose_vector.xml
@@ -1,118 +1,32 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Create Pose Vector">
   <!--//////////-->
-  <BehaviorTree
-    ID="Create Pose Vector"
-    _favorite="false"
-    _subtreeOnly="true"
-    _description="Creates a sequence of stamped poses and adds them to a vector"
-    target_poses="{target_poses}"
-  >
+  <BehaviorTree ID="Create Pose Vector" _favorite="false" _subtreeOnly="true" _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;-1;0;0"
-        position_xyz="0.75;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;-1;0;0"
-        position_xyz="0.5;0.5;0.8"
-        stamped_pose="{stamped_pose}"
-        reference_frame="world"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="0.25;0.5;0.7"
-        stamped_pose="{stamped_pose}"
-        reference_frame="world"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="0;0.5;0.6"
-        stamped_pose="{stamped_pose}"
-        reference_frame="world"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="-0.25;0.5;0.7"
-        stamped_pose="{stamped_pose}"
-        reference_frame="world"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="-0.5;0.5;0.8"
-        stamped_pose="{stamped_pose}"
-        reference_frame="world"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;-1;0;0"
-        position_xyz="-0.75;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;-1;0;0"
-        position_xyz="-1;0.5;1"
-        stamped_pose="{stamped_pose}"
-        reference_frame="world"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;-1;0;0" position_xyz="0.75;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;-1;0;0" position_xyz="0.5;0.5;0.8" stamped_pose="{stamped_pose}" reference_frame="world"/>
+      <Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;-1;0;0" position_xyz="0.25;0.5;0.7" stamped_pose="{stamped_pose}" reference_frame="world"/>
+      <Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;-1;0;0" position_xyz="0;0.5;0.6" stamped_pose="{stamped_pose}" reference_frame="world"/>
+      <Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;-1;0;0" position_xyz="-0.25;0.5;0.7" stamped_pose="{stamped_pose}" reference_frame="world"/>
+      <Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;-1;0;0" position_xyz="-0.5;0.5;0.8" stamped_pose="{stamped_pose}" reference_frame="world"/>
+      <Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;-1;0;0" position_xyz="-0.75;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;-1;0;0" position_xyz="-1;0.5;1" stamped_pose="{stamped_pose}" reference_frame="world"/>
+      <Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Create Pose Vector">
-      <output_port name="target_poses" default="{target_poses}" />
+      <output_port name="target_poses" default="{target_poses}"/>
       <MetadataFields>
-        <Metadata subcategory="Vector Handling" />
-        <Metadata runnable="false" />
+        <Metadata subcategory="Vector Handling"/>
+        <Metadata runnable="false"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/cycle_between_waypoints.xml b/src/lab_sim/objectives/cycle_between_waypoints.xml
index 478c75ad..f649a143 100644
--- a/src/lab_sim/objectives/cycle_between_waypoints.xml
+++ b/src/lab_sim/objectives/cycle_between_waypoints.xml
@@ -1,30 +1,17 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Cycle Between Waypoints">
-  <BehaviorTree
-    ID="Cycle Between Waypoints"
-    _description="Example of repeatedly moving between two saved poses."
-    _favorite="false"
-  >
+  <BehaviorTree ID="Cycle Between Waypoints" _description="Example of repeatedly moving between two saved poses." _favorite="false">
     <Decorator ID="KeepRunningUntilFailure">
       <Control ID="Sequence">
-        <SubTree
-          ID="Move to Waypoint"
-          waypoint_name="Above Pick Cube"
-          joint_group_name="manipulator"
-          controller_names="/joint_trajectory_controller"
-        />
-        <SubTree
-          ID="Move to Waypoint"
-          waypoint_name="Look at Table"
-          joint_group_name="manipulator"
-          controller_names="/joint_trajectory_controller"
-        />
+        <SubTree ID="Move to Waypoint" waypoint_name="Above Pick Cube" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
+        <SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
       </Control>
     </Decorator>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Cycle Between Waypoints">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/force_relaxation.xml b/src/lab_sim/objectives/force_relaxation.xml
index f137daea..a5c91156 100644
--- a/src/lab_sim/objectives/force_relaxation.xml
+++ b/src/lab_sim/objectives/force_relaxation.xml
@@ -1,57 +1,29 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Force Relaxation">
   <!--//////////-->
-  <BehaviorTree
-    ID="Force Relaxation"
-    _description="Relaxes the force sensed at the force/torque sensor by moving the robot tip in the direction of the sensed force."
-    _favorite="false"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Force Relaxation" _description="Relaxes the force sensed at the force/torque sensor by moving the robot tip in the direction of the sensed force." _favorite="false" _subtreeOnly="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedTwist"
-        reference_frame="{tip_link}"
-        stamped_twist="{zero_twist}"
-      />
-      <Action
-        ID="CreateStampedWrench"
-        reference_frame="{tip_link}"
-        stamped_wrench="{zero_wrench}"
-      />
+      <Action ID="CreateStampedTwist" reference_frame="{tip_link}" stamped_twist="{zero_twist}"/>
+      <Action ID="CreateStampedWrench" reference_frame="{tip_link}" stamped_wrench="{zero_wrench}"/>
       <Decorator ID="ForceSuccess">
         <Decorator ID="Timeout" msec="{relaxation_time_ms}">
-          <Action
-            ID="PublishVelocityForceCommand"
-            wrench_gain="{wrench_gain}"
-            force_controlled_axes="1;1;1;0;0;0"
-            publish_rate="50"
-            twist_stamped="{zero_twist}"
-            wrench_stamped="{zero_wrench}"
-          />
+          <Action ID="PublishVelocityForceCommand" wrench_gain="{wrench_gain}" force_controlled_axes="1;1;1;0;0;0" publish_rate="50" twist_stamped="{zero_twist}" wrench_stamped="{zero_wrench}"/>
         </Decorator>
       </Decorator>
       <Decorator ID="ForceSuccess">
         <Decorator ID="Timeout" msec="10">
-          <Action
-            ID="PublishVelocityForceCommand"
-            wrench_gain="{wrench_gain}"
-            force_controlled_axes="0;0;0;0;0;0"
-            publish_rate="50"
-            velocity_controlled_axes="1;1;1;0;0;0"
-            wrench_stamped="{zero_wrench}"
-            twist_stamped="{zero_twist}"
-          />
+          <Action ID="PublishVelocityForceCommand" wrench_gain="{wrench_gain}" force_controlled_axes="0;0;0;0;0;0" publish_rate="50" velocity_controlled_axes="1;1;1;0;0;0" wrench_stamped="{zero_wrench}" twist_stamped="{zero_twist}"/>
         </Decorator>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Force Relaxation">
-      <input_port name="wrench_gain" default="0.01" />
-      <input_port name="relaxation_time_ms" default="500" />
-      <input_port name="tip_link" default="grasp_link" />
+      <input_port name="wrench_gain" default="0.01"/>
+      <input_port name="relaxation_time_ms" default="500"/>
+      <input_port name="tip_link" default="grasp_link"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Controls" />
+        <Metadata subcategory="Motion - Controls"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/get_april_tag_pose_from_image.xml b/src/lab_sim/objectives/get_april_tag_pose_from_image.xml
index bee74230..dd49305a 100644
--- a/src/lab_sim/objectives/get_april_tag_pose_from_image.xml
+++ b/src/lab_sim/objectives/get_april_tag_pose_from_image.xml
@@ -1,55 +1,24 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Get AprilTag Pose from Image">
-  <BehaviorTree
-    ID="Get AprilTag Pose from Image"
-    _subtreeOnly="true"
-    _description="Takes an image snapshot from the wrist camera and attempts to detect an april tag for pose estimation."
-  >
+  <BehaviorTree ID="Get AprilTag Pose from Image" _subtreeOnly="true" _description="Takes an image snapshot from the wrist camera and attempts to detect an april tag for pose estimation.">
     <Control ID="Sequence">
       <!--Get tag pose estimate in world frame-->
-      <Action
-        ID="DetectAprilTags"
-        image="{image}"
-        camera_info="{camera_info}"
-        parameters="{parameters}"
-        detections="{detections}"
-      />
-      <Action
-        ID="GetImage"
-        topic_name="/wrist_camera/color"
-        message_out="{image}"
-        timeout_sec="5.000000"
-      />
-      <Action
-        ID="DetectAprilTags"
-        image="{image}"
-        camera_info="{camera_info}"
-        parameters="{parameters}"
-        detections="{detections}"
-      />
-      <Action
-        ID="GetDetectionPose"
-        detection_pose="{detection_pose}"
-        detections="{detections}"
-        target_id="-1"
-      />
-      <Action
-        ID="TransformPoseFrame"
-        input_pose="{detection_pose}"
-        output_pose="{detection_pose}"
-        target_frame_id="world"
-      />
+      <Action ID="DetectAprilTags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/>
+      <Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}" timeout_sec="5.000000"/>
+      <Action ID="DetectAprilTags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/>
+      <Action ID="GetDetectionPose" detection_pose="{detection_pose}" detections="{detections}" target_id="-1"/>
+      <Action ID="TransformPoseFrame" input_pose="{detection_pose}" output_pose="{detection_pose}" target_frame_id="world"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Get AprilTag Pose from Image">
-      <input_port name="camera_info" default="{camera_info}" />
-      <output_port name="detection_pose" default="{detection_pose}" />
-      <input_port name="image" default="{image}" />
-      <input_port name="parameters" default="{parameters}" />
+      <input_port name="camera_info" default="{camera_info}"/>
+      <output_port name="detection_pose" default="{detection_pose}"/>
+      <input_port name="image" default="{image}"/>
+      <input_port name="parameters" default="{parameters}"/>
       <MetadataFields>
-        <Metadata subcategory="Perception - 2D Image" />
-        <Metadata runnable="false" />
+        <Metadata subcategory="Perception - 2D Image"/>
+        <Metadata runnable="false"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/get_transform_frame_pose.xml b/src/lab_sim/objectives/get_transform_frame_pose.xml
index 13423c55..e678ee08 100644
--- a/src/lab_sim/objectives/get_transform_frame_pose.xml
+++ b/src/lab_sim/objectives/get_transform_frame_pose.xml
@@ -1,38 +1,19 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Get Transform Frame Pose">
   <!--//////////-->
-  <BehaviorTree
-    ID="Get Transform Frame Pose"
-    _description="Returns the pose of the requested frame in the world frame."
-    _favorite="false"
-    _subtreeOnly="true"
-    name="Returns the pose of the requested frame in the world frame"
-  >
+  <BehaviorTree ID="Get Transform Frame Pose" _description="Returns the pose of the requested frame in the world frame." _favorite="false" _subtreeOnly="true" name="Returns the pose of the requested frame in the world frame">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="{requested_frame}"
-        stamped_pose="{identity_pose}"
-        name="Make an identity pose to be transformed"
-        orientation_xyzw="0.; 0.; 0.; 1."
-        position_xyz="0.;0.;0."
-      />
-      <Action
-        ID="TransformPoseFrame"
-        input_pose="{identity_pose}"
-        output_pose="{requested_pose}"
-        target_frame_id="world"
-        name="Use identity pose to get requested pose"
-      />
+      <Action ID="CreateStampedPose" reference_frame="{requested_frame}" stamped_pose="{identity_pose}" name="Make an identity pose to be transformed" orientation_xyzw="0.; 0.; 0.; 1." position_xyz="0.;0.;0."/>
+      <Action ID="TransformPoseFrame" input_pose="{identity_pose}" output_pose="{requested_pose}" target_frame_id="world" name="Use identity pose to get requested pose"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Get Transform Frame Pose">
-      <inout_port name="requested_frame" default="" />
-      <inout_port name="requested_pose" default="" />
+      <inout_port name="requested_frame" default=""/>
+      <inout_port name="requested_pose" default=""/>
       <MetadataFields>
-        <Metadata subcategory="Pose Handling" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Pose Handling"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/grasp_object_from_text_prompt.xml b/src/lab_sim/objectives/grasp_object_from_text_prompt.xml
index f472ea64..b030f8fc 100644
--- a/src/lab_sim/objectives/grasp_object_from_text_prompt.xml
+++ b/src/lab_sim/objectives/grasp_object_from_text_prompt.xml
@@ -1,132 +1,35 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Grasp Object from Text Prompt">
   <!--//////////-->
-  <BehaviorTree
-    ID="Grasp Object from Text Prompt"
-    _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"
-  >
+  <BehaviorTree ID="Grasp Object from Text Prompt" _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">
     <Control ID="Sequence">
-      <SubTree ID="Open Gripper" _collapsed="true" />
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_link_names="grasp_link"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        waypoint_name="Above Pick Cube"
-      />
-      <SubTree
-        ID="Segment Point Cloud from Text Prompt Subtree"
-        _collapsed="false"
-        camera_topic_name="/wrist_camera/camera_info"
-        decoder_model_path="models/decoder.onnx"
-        encoder_model_path="models/sam2_hiera_large_encoder.onnx"
-        image_topic_name="/wrist_camera/color"
-        masks_visualization_topic="/masks_visualization"
-        model_package="lab_sim"
-        points_topic_name="/wrist_camera/points"
-        clip_model_path="models/clip.onnx"
-        clipseg_model_path="models/clipseg.onnx"
-        erosion_size="2"
-        prompts="an object"
-        threshold="0.28"
-        masks3d="{masks3d}"
-        point_cloud="{point_cloud}"
-        point_cloud_vector="{point_cloud_vector}"
-      />
-      <Decorator
-        ID="ForEach"
-        index="{index}"
-        out="{point_cloud_fragment}"
-        vector_in="{point_cloud_vector}"
-      >
+      <SubTree ID="Open Gripper" _collapsed="true"/>
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0" waypoint_name="Above Pick Cube"/>
+      <SubTree ID="Segment Point Cloud from Text Prompt Subtree" _collapsed="false" camera_topic_name="/wrist_camera/camera_info" decoder_model_path="models/decoder.onnx" encoder_model_path="models/sam2_hiera_large_encoder.onnx" image_topic_name="/wrist_camera/color" masks_visualization_topic="/masks_visualization" model_package="lab_sim" points_topic_name="/wrist_camera/points" clip_model_path="models/clip.onnx" clipseg_model_path="models/clipseg.onnx" erosion_size="2" prompts="an object" threshold="0.28" masks3d="{masks3d}" point_cloud="{point_cloud}" point_cloud_vector="{point_cloud_vector}"/>
+      <Decorator ID="ForEach" index="{index}" out="{point_cloud_fragment}" vector_in="{point_cloud_vector}">
         <Control ID="Sequence">
-          <Action
-            ID="GetCentroidFromPointCloud"
-            output_pose="{centroid_pose}"
-            point_cloud="{point_cloud_fragment}"
-          />
-          <Action
-            ID="TransformPose"
-            input_pose="{centroid_pose}"
-            output_pose="{grasp_pose}"
-            quaternion_xyzw="0;0;1.0;0.0"
-          />
-          <Action
-            ID="AddPoseStampedToVector"
-            input="{grasp_pose}"
-            vector="{grasp_poses}"
-          />
+          <Action ID="GetCentroidFromPointCloud" output_pose="{centroid_pose}" point_cloud="{point_cloud_fragment}"/>
+          <Action ID="TransformPose" input_pose="{centroid_pose}" output_pose="{grasp_pose}" quaternion_xyzw="0;0;1.0;0.0"/>
+          <Action ID="AddPoseStampedToVector" input="{grasp_pose}" vector="{grasp_poses}"/>
         </Control>
       </Decorator>
       <Control ID="Sequence">
-        <Action
-          ID="InitializeMTCTask"
-          task_id="grasp"
-          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          task="{mtc_task}"
-        />
-        <Action
-          ID="SetupMTCCurrentState"
-          task="{mtc_task}"
-          skip_collision_check="false"
-        />
-        <Action
-          ID="SetupMTCConnectWithTrajectory"
-          planner_interface="pro_rrt"
-          planning_group_name="manipulator"
-          task="{mtc_task}"
-          constraints="{constraints}"
-        />
-        <Action
-          ID="SetupMTCBatchPoseIK"
-          end_effector_group="moveit_ee"
-          end_effector_link="grasp_link"
-          ik_timeout_s="0.01"
-          max_ik_solutions="1"
-          monitored_stage="current state"
-          target_poses="{grasp_poses}"
-          task="{mtc_task}"
-        />
-        <Action
-          ID="PlanMTCTask"
-          solution="{grasp_solution}"
-          task="{mtc_task}"
-        />
-        <Action
-          ID="ExecuteMTCTask"
-          goal_duration_tolerance="-1.000000"
-          solution="{grasp_solution}"
-        />
+        <Action ID="InitializeMTCTask" task_id="grasp" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{mtc_task}"/>
+        <Action ID="SetupMTCCurrentState" task="{mtc_task}" skip_collision_check="false"/>
+        <Action ID="SetupMTCConnectWithTrajectory" planner_interface="pro_rrt" planning_group_name="manipulator" task="{mtc_task}" constraints="{constraints}"/>
+        <Action ID="SetupMTCBatchPoseIK" end_effector_group="moveit_ee" end_effector_link="grasp_link" ik_timeout_s="0.01" max_ik_solutions="1" monitored_stage="current state" target_poses="{grasp_poses}" task="{mtc_task}"/>
+        <Action ID="PlanMTCTask" solution="{grasp_solution}" task="{mtc_task}"/>
+        <Action ID="ExecuteMTCTask" goal_duration_tolerance="-1.000000" solution="{grasp_solution}"/>
       </Control>
-      <SubTree ID="Close Gripper" _collapsed="true" />
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_link_names="grasp_link"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        waypoint_name="Above Pick Cube"
-      />
+      <SubTree ID="Close Gripper" _collapsed="true"/>
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0" waypoint_name="Above Pick Cube"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Grasp Object from Text Prompt">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/grasp_planning.xml b/src/lab_sim/objectives/grasp_planning.xml
index b8bc9c91..c64d6e8d 100644
--- a/src/lab_sim/objectives/grasp_planning.xml
+++ b/src/lab_sim/objectives/grasp_planning.xml
@@ -1,112 +1,40 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Grasp Planning">
   <!--//////////-->
-  <BehaviorTree
-    ID="Grasp Planning"
-    _description="Find a feasible grasp pose out of a set of manually-defined grasp candidates, and move there"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Grasp Planning" _description="Find a feasible grasp pose out of a set of manually-defined grasp candidates, and move there" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Clear Snapshot" _collapsed="true" />
-      <SubTree ID="Look at table" _collapsed="true" />
+      <SubTree ID="Clear Snapshot" _collapsed="true"/>
+      <SubTree ID="Look at table" _collapsed="true"/>
       <!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
-      <SubTree ID="Open Gripper" _collapsed="true" />
-      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
+      <SubTree ID="Open Gripper" _collapsed="true"/>
+      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
       <Control ID="Sequence">
         <!--Define and visualize two grasp candidates-->
-        <Action
-          ID="CreateStampedPose"
-          orientation_xyzw="0;1;0;0"
-          position_xyz="0.01;.75;0.515"
-          stamped_pose="{grasp_candidate_1}"
-          reference_frame="world"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{grasp_candidate_1}"
-          vector="{grasp_candidates}"
-        />
-        <Action
-          ID="VisualizePose"
-          marker_lifetime="0.000000"
-          marker_name="grasp_candidate_1"
-          marker_size="0.100000"
-          pose="{grasp_candidate_1}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          orientation_xyzw="0;0.9848078;0.1736482;0"
-          position_xyz="0.01;.75;0.515"
-          stamped_pose="{grasp_candidate_2}"
-          reference_frame="world"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{grasp_candidate_2}"
-          vector="{grasp_candidates}"
-        />
-        <Action
-          ID="VisualizePose"
-          marker_lifetime="0.000000"
-          marker_name="grasp_candidate_2"
-          marker_size="0.100000"
-          pose="{grasp_candidate_2}"
-        />
+        <Action ID="CreateStampedPose" orientation_xyzw="0;1;0;0" position_xyz="0.01;.75;0.515" stamped_pose="{grasp_candidate_1}" reference_frame="world"/>
+        <Action ID="AddPoseStampedToVector" input="{grasp_candidate_1}" vector="{grasp_candidates}"/>
+        <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="grasp_candidate_1" marker_size="0.100000" pose="{grasp_candidate_1}"/>
+        <Action ID="CreateStampedPose" orientation_xyzw="0;0.9848078;0.1736482;0" position_xyz="0.01;.75;0.515" stamped_pose="{grasp_candidate_2}" reference_frame="world"/>
+        <Action ID="AddPoseStampedToVector" input="{grasp_candidate_2}" vector="{grasp_candidates}"/>
+        <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="grasp_candidate_2" marker_size="0.100000" pose="{grasp_candidate_2}"/>
       </Control>
       <Control ID="Sequence">
         <!--Find a feasible grasp pose and move there-->
-        <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}"
-          skip_collision_check="false"
-        />
-        <Action
-          ID="SetupMTCConnectWithTrajectory"
-          constraints="{constraints}"
-          planner_interface="moveit_default"
-          planning_group_name="manipulator"
-          task="{pick_object_task}"
-        />
-        <Action
-          ID="SetupMTCBatchPoseIK"
-          end_effector_group="moveit_ee"
-          end_effector_link="grasp_link"
-          ik_timeout_s="0.01"
-          max_ik_solutions="1"
-          monitored_stage="current state"
-          target_poses="{grasp_candidates}"
-          task="{pick_object_task}"
-        />
-        <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}"
-          goal_duration_tolerance="-1.000000"
-        />
+        <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}" skip_collision_check="false"/>
+        <Action ID="SetupMTCConnectWithTrajectory" constraints="{constraints}" planner_interface="moveit_default" planning_group_name="manipulator" task="{pick_object_task}"/>
+        <Action ID="SetupMTCBatchPoseIK" end_effector_group="moveit_ee" end_effector_link="grasp_link" ik_timeout_s="0.01" max_ik_solutions="1" monitored_stage="current state" target_poses="{grasp_candidates}" task="{pick_object_task}"/>
+        <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}" goal_duration_tolerance="-1.000000"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Grasp Planning">
       <MetadataFields>
-        <Metadata subcategory="Motion - Planning" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Motion - Planning"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/grasp_pose_tuning_with_april_tag.xml b/src/lab_sim/objectives/grasp_pose_tuning_with_april_tag.xml
index c1169b69..8698b8e8 100644
--- a/src/lab_sim/objectives/grasp_pose_tuning_with_april_tag.xml
+++ b/src/lab_sim/objectives/grasp_pose_tuning_with_april_tag.xml
@@ -1,112 +1,41 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Grasp Pose Tuning With April Tag">
   <!--//////////-->
-  <BehaviorTree
-    ID="Grasp Pose Tuning With April Tag"
-    _description="Moves the gripper to the April Tag and then prompts the user to teleop to a grasp pose. Saves the resulting pose to a YAML file."
-    _favorite="false"
-  >
+  <BehaviorTree ID="Grasp Pose Tuning With April Tag" _description="Moves the gripper to the April Tag and then prompts the user to teleop to a grasp pose. Saves the resulting pose to a YAML file." _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Open Gripper" />
+      <SubTree ID="Open Gripper"/>
       <!--Be sure that no previous obstacle left in the scene interferes with planning.-->
-      <SubTree ID="Clear Previous Obstacles" _collapsed="true" />
+      <SubTree ID="Clear Previous Obstacles" _collapsed="true"/>
       <!--Look at table, find an april tag.-->
-      <SubTree ID="Look at table" _collapsed="true" />
-      <SubTree ID="Clear Previous Obstacles" _collapsed="true" />
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
-      <SubTree
-        ID="Take Wrist Camera Image"
-        _collapsed="true"
-        wrist_camera_image="{wrist_camera_image}"
-        wrist_camera_info="{wrist_camera_info}"
-        wrist_camera_parameters="{wrist_camera_parameters}"
-      />
-      <SubTree
-        ID="Get AprilTag Pose from Image"
-        image="{wrist_camera_image}"
-        camera_info="{wrist_camera_info}"
-        parameters="{wrist_camera_parameters}"
-        _collapsed="true"
-        detection_pose="{detection_pose}"
-      />
+      <SubTree ID="Look at table" _collapsed="true"/>
+      <SubTree ID="Clear Previous Obstacles" _collapsed="true"/>
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
+      <SubTree ID="Take Wrist Camera Image" _collapsed="true" wrist_camera_image="{wrist_camera_image}" wrist_camera_info="{wrist_camera_info}" wrist_camera_parameters="{wrist_camera_parameters}"/>
+      <SubTree ID="Get AprilTag Pose from Image" image="{wrist_camera_image}" camera_info="{wrist_camera_info}" parameters="{wrist_camera_parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
       <!--Move in for a closer look.-->
-      <Action
-        ID="TransformPose"
-        output_pose="{view_pose}"
-        input_pose="{detection_pose}"
-        quaternion_xyzw="0;0;0;1"
-        translation_xyz="0;-0.1;-0.15"
-      />
-      <SubTree
-        ID="Plan Move To Pose"
-        target_pose="{view_pose}"
-        move_to_pose_solution="{move_to_pose_solution}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{move_to_pose_solution}"
-        goal_duration_tolerance="-1.000000"
-      />
+      <Action ID="TransformPose" output_pose="{view_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;-0.1;-0.15"/>
+      <SubTree ID="Plan Move To Pose" target_pose="{view_pose}" move_to_pose_solution="{move_to_pose_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" goal_duration_tolerance="-1.000000"/>
       <!--Locate the april tag again more precisely.-->
-      <SubTree ID="Clear Previous Obstacles" _collapsed="true" />
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
-      <SubTree
-        ID="Take Wrist Camera Image"
-        _collapsed="true"
-        wrist_camera_image="{wrist_camera_image}"
-        wrist_camera_info="{wrist_camera_info}"
-        wrist_camera_parameters="{wrist_camera_parameters}"
-      />
-      <SubTree
-        ID="Get AprilTag Pose from Image"
-        image="{wrist_camera_image}"
-        camera_info="{wrist_camera_info}"
-        parameters="{wrist_camera_parameters}"
-        _collapsed="true"
-        detection_pose="{detection_pose}"
-      />
+      <SubTree ID="Clear Previous Obstacles" _collapsed="true"/>
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
+      <SubTree ID="Take Wrist Camera Image" _collapsed="true" wrist_camera_image="{wrist_camera_image}" wrist_camera_info="{wrist_camera_info}" wrist_camera_parameters="{wrist_camera_parameters}"/>
+      <SubTree ID="Get AprilTag Pose from Image" image="{wrist_camera_image}" camera_info="{wrist_camera_info}" parameters="{wrist_camera_parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
       <!--Now that we know where the april tag is, prompt the user to position the gripper to pick it up.-->
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="detection pose"
-        marker_size="0.100000"
-        pose="{detection_pose}"
-      />
-      <SubTree
-        ID="Request Teleoperation"
-        _collapsed="true"
-        enable_user_interaction="true"
-        initial_teleop_mode="3"
-        user_interaction_prompt="Teleoperate end effector to grasp pose"
-      />
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="detection pose" marker_size="0.100000" pose="{detection_pose}"/>
+      <SubTree ID="Request Teleoperation" _collapsed="true" enable_user_interaction="true" initial_teleop_mode="3" user_interaction_prompt="Teleoperate end effector to grasp pose"/>
       <!--Compute the offset from the april tag to the specified gripper pose and save that to yaml.-->
-      <SubTree
-        ID="Get Transform Frame Pose"
-        _collapsed="true"
-        requested_frame="grasp_link"
-        requested_pose="{gripper_pose}"
-      />
-      <Action
-        ID="CalculatePoseOffset"
-        destination_pose="{gripper_pose}"
-        source_pose="{detection_pose}"
-        source_to_destination_pose="{relative_pose}"
-      />
-      <Action
-        ID="SavePoseToYaml"
-        message="{relative_pose}"
-        yaml_filename="relative_grasp_pose"
-        namespace="RelativePose"
-      />
+      <SubTree ID="Get Transform Frame Pose" _collapsed="true" requested_frame="grasp_link" requested_pose="{gripper_pose}"/>
+      <Action ID="CalculatePoseOffset" destination_pose="{gripper_pose}" source_pose="{detection_pose}" source_to_destination_pose="{relative_pose}"/>
+      <Action ID="SavePoseToYaml" message="{relative_pose}" yaml_filename="relative_grasp_pose" namespace="RelativePose"/>
       <!--Now that it is saved to yaml, it can be loaded as a grasp offset. Test with the "Grasp Pose Using Yaml" objective.-->
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Grasp Pose Tuning With April Tag">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/grasp_pose_using_yaml.xml b/src/lab_sim/objectives/grasp_pose_using_yaml.xml
index baf168f9..2ab16fc9 100644
--- a/src/lab_sim/objectives/grasp_pose_using_yaml.xml
+++ b/src/lab_sim/objectives/grasp_pose_using_yaml.xml
@@ -1,131 +1,44 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Grasp Pose Using Yaml">
   <!--//////////-->
-  <BehaviorTree
-    ID="Grasp Pose Using Yaml"
-    _description="Reads a YAML file for grasp pose offset and picks up an object that has an AprilTag marker."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Grasp Pose Using Yaml" _description="Reads a YAML file for grasp pose offset and picks up an object that has an AprilTag marker." _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Open Gripper" />
+      <SubTree ID="Open Gripper"/>
       <!--Be sure that no previous obstacle left in the scene interferes with planning.-->
-      <SubTree ID="Clear Previous Obstacles" _collapsed="true" />
+      <SubTree ID="Clear Previous Obstacles" _collapsed="true"/>
       <!--Look at table, find an april tag.-->
-      <SubTree ID="Look at table" _collapsed="true" />
-      <SubTree ID="Clear Previous Obstacles" _collapsed="true" />
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
-      <SubTree
-        ID="Take Wrist Camera Image"
-        _collapsed="true"
-        wrist_camera_image="{wrist_camera_image}"
-        wrist_camera_info="{wrist_camera_info}"
-        wrist_camera_parameters="{wrist_camera_parameters}"
-      />
-      <SubTree
-        ID="Get AprilTag Pose from Image"
-        image="{wrist_camera_image}"
-        camera_info="{wrist_camera_info}"
-        parameters="{wrist_camera_parameters}"
-        _collapsed="true"
-        detection_pose="{detection_pose}"
-      />
+      <SubTree ID="Look at table" _collapsed="true"/>
+      <SubTree ID="Clear Previous Obstacles" _collapsed="true"/>
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
+      <SubTree ID="Take Wrist Camera Image" _collapsed="true" wrist_camera_image="{wrist_camera_image}" wrist_camera_info="{wrist_camera_info}" wrist_camera_parameters="{wrist_camera_parameters}"/>
+      <SubTree ID="Get AprilTag Pose from Image" image="{wrist_camera_image}" camera_info="{wrist_camera_info}" parameters="{wrist_camera_parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
       <!--Move in for a closer look.-->
-      <Action
-        ID="TransformPose"
-        output_pose="{view_pose}"
-        input_pose="{detection_pose}"
-        quaternion_xyzw="0;0;0;1"
-        translation_xyz="0;-0.1;-0.15"
-      />
-      <SubTree
-        ID="Plan Move To Pose"
-        target_pose="{view_pose}"
-        move_to_pose_solution="{move_to_pose_solution}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{move_to_pose_solution}"
-        goal_duration_tolerance="-1.000000"
-      />
+      <Action ID="TransformPose" output_pose="{view_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;-0.1;-0.15"/>
+      <SubTree ID="Plan Move To Pose" target_pose="{view_pose}" move_to_pose_solution="{move_to_pose_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" goal_duration_tolerance="-1.000000"/>
       <!--Locate the april tag again more precisely.-->
-      <SubTree ID="Clear Previous Obstacles" _collapsed="true" />
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
-      <SubTree
-        ID="Take Wrist Camera Image"
-        _collapsed="true"
-        wrist_camera_image="{wrist_camera_image}"
-        wrist_camera_info="{wrist_camera_info}"
-        wrist_camera_parameters="{wrist_camera_parameters}"
-      />
-      <SubTree
-        ID="Get AprilTag Pose from Image"
-        image="{wrist_camera_image}"
-        camera_info="{wrist_camera_info}"
-        parameters="{wrist_camera_parameters}"
-        _collapsed="true"
-        detection_pose="{detection_pose}"
-      />
+      <SubTree ID="Clear Previous Obstacles" _collapsed="true"/>
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
+      <SubTree ID="Take Wrist Camera Image" _collapsed="true" wrist_camera_image="{wrist_camera_image}" wrist_camera_info="{wrist_camera_info}" wrist_camera_parameters="{wrist_camera_parameters}"/>
+      <SubTree ID="Get AprilTag Pose from Image" image="{wrist_camera_image}" camera_info="{wrist_camera_info}" parameters="{wrist_camera_parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
       <!--Load the gripper offset from yaml and combine with detected pose.-->
-      <Action
-        ID="LoadObjectiveParameters"
-        parameters="{relative_pose_parameters}"
-        config_file_name="relative_grasp_pose.yaml"
-      />
-      <Action
-        ID="TransformPoseFromYaml"
-        input_pose="{detection_pose}"
-        output_pose="{offset_pose}"
-        pose_parameters="{relative_pose_parameters}"
-        parameter_namespace="RelativePose"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="pose"
-        marker_size="0.100000"
-        pose="{offset_pose}"
-      />
+      <Action ID="LoadObjectiveParameters" parameters="{relative_pose_parameters}" config_file_name="relative_grasp_pose.yaml"/>
+      <Action ID="TransformPoseFromYaml" input_pose="{detection_pose}" output_pose="{offset_pose}" pose_parameters="{relative_pose_parameters}" parameter_namespace="RelativePose"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{offset_pose}"/>
       <!--Now pick the cube using the offset_pose.-->
-      <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}"
-        skip_collision_check="false"
-      />
-      <Action
-        ID="SetupMTCPickObject"
-        grasp_pose="{offset_pose}"
-        task="{pick_object_task}"
-        parameters="{parameters}"
-        monitored_stage="current state"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{pick_object_solution}"
-        task="{pick_object_task}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{pick_object_solution}"
-        goal_duration_tolerance="-1.000000"
-      />
+      <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}" skip_collision_check="false"/>
+      <Action ID="SetupMTCPickObject" grasp_pose="{offset_pose}" task="{pick_object_task}" parameters="{parameters}" monitored_stage="current state"/>
+      <Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
+      <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" goal_duration_tolerance="-1.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Grasp Pose Using Yaml">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/joint_diagnostic.xml b/src/lab_sim/objectives/joint_diagnostic.xml
index 8e7ecccc..4ee20535 100644
--- a/src/lab_sim/objectives/joint_diagnostic.xml
+++ b/src/lab_sim/objectives/joint_diagnostic.xml
@@ -1,36 +1,15 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Joint Diagnostic">
   <!--//////////-->
-  <BehaviorTree
-    ID="Joint Diagnostic"
-    _description="Example of cycling through the min and max limits of a single joint, for hardware testing"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Joint Diagnostic" _description="Example of cycling through the min and max limits of a single joint, for hardware testing" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <!--Setup by moving home-->
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Look at Table"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller"
-        link_padding="0.0"
-      />
+      <SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller" link_padding="0.0"/>
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
           <!--If you use two waypoints that have identical joint values, except for one joint, you can use this Objective as a way to test a single joint repeatedly. This is useful for things like hardware diagnostics and hardware bringup.-->
-          <SubTree
-            ID="Move to Waypoint"
-            waypoint_name="Wrist 2 Max"
-            joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller"
-            link_padding="0.0"
-          />
-          <SubTree
-            ID="Move to Waypoint"
-            waypoint_name="Wrist 2 Min"
-            joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller"
-            link_padding="0.0"
-          />
+          <SubTree ID="Move to Waypoint" waypoint_name="Wrist 2 Max" joint_group_name="manipulator" controller_names="/joint_trajectory_controller" link_padding="0.0"/>
+          <SubTree ID="Move to Waypoint" waypoint_name="Wrist 2 Min" joint_group_name="manipulator" controller_names="/joint_trajectory_controller" link_padding="0.0"/>
         </Control>
       </Decorator>
     </Control>
@@ -38,7 +17,7 @@
   <TreeNodesModel>
     <SubTree ID="Joint Diagnostic">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml b/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml
index cfe0beed..8deec852 100644
--- a/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml
+++ b/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml
@@ -1,61 +1,22 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Load and Execute Joint Trajectory">
   <!--//////////-->
-  <BehaviorTree
-    ID="Load and Execute Joint Trajectory"
-    _description="Load a trajectory that was saved as a YAML file"
-    _favorite="true"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Load and Execute Joint Trajectory" _description="Load a trajectory that was saved as a YAML file" _favorite="true" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <!--Make sure to move the robot to the same starting position as the saved trajectory starts, otherwise the joint_trajectory_controller will use joint interpolation to align the robot with the path-->
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        waypoint_name="Workspace Right"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-      />
-      <Action
-        ID="LoadJointTrajectoryFromYaml"
-        file_path="joint_trajectory.yaml"
-        output="{joint_trajectory_msg}"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Workspace Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0"/>
+      <Action ID="LoadJointTrajectoryFromYaml" file_path="joint_trajectory.yaml" output="{joint_trajectory_msg}"/>
       <!--Validate the loaded trajectory with the current planning scene to check for collisions before executing-->
-      <Action
-        ID="GetCurrentPlanningScene"
-        planning_scene_msg="{planning_scene}"
-      />
-      <Action
-        ID="ValidateTrajectory"
-        cartesian_space_step="0.020000"
-        joint_space_step="0.100000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-        planning_group_name="manipulator"
-        planning_scene_msg="{planning_scene}"
-        debug_solution="{debug_solution}"
-      />
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-        goal_duration_tolerance="-1.000000"
-        goal_position_tolerance="0.000000"
-        goal_time_tolerance="0.000000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene}"/>
+      <Action ID="ValidateTrajectory" cartesian_space_step="0.020000" joint_space_step="0.100000" joint_trajectory_msg="{joint_trajectory_msg}" planning_group_name="manipulator" planning_scene_msg="{planning_scene}" debug_solution="{debug_solution}"/>
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Load and Execute Joint Trajectory">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Basic Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml b/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml
index c104fbff..445e093a 100644
--- a/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml
+++ b/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml
@@ -1,38 +1,20 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Load Mesh as Green Point Cloud">
   <!--//////////-->
-  <BehaviorTree
-    ID="Load Mesh as Green Point Cloud"
-    _description="Loads a point cloud from a file and colors it green."
-    _favorite="false"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Load Mesh as Green Point Cloud" _description="Loads a point cloud from a file and colors it green." _favorite="false" _subtreeOnly="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="LoadPointCloudFromFile"
-        color="0;255;0"
-        file_path="~/user_ws/install/lab_sim/share/lab_sim/description/assets/Cube.stl"
-        point_cloud="{point_cloud}"
-      />
-      <Action
-        ID="TransformPointCloudFrame"
-        input_cloud="{point_cloud}"
-        output_cloud="{point_cloud}"
-      />
-      <Action
-        ID="TransformPointCloud"
-        input_cloud="{point_cloud}"
-        transform_pose="{initial_pose}"
-      />
+      <Action ID="LoadPointCloudFromFile" color="0;255;0" file_path="~/user_ws/install/lab_sim/share/lab_sim/description/assets/Cube.stl" point_cloud="{point_cloud}"/>
+      <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" output_cloud="{point_cloud}"/>
+      <Action ID="TransformPointCloud" input_cloud="{point_cloud}" transform_pose="{initial_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Load Mesh as Green Point Cloud">
-      <output_port name="point_cloud" default="{green_cloud}" />
-      <input_port name="_collapsed" default="true" />
-      <input_port name="initial_pose" default="{stamped_pose}" />
+      <output_port name="point_cloud" default="{green_cloud}"/>
+      <input_port name="_collapsed" default="true"/>
+      <input_port name="initial_pose" default="{stamped_pose}"/>
       <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
+        <Metadata subcategory="Perception - 3D Point Cloud"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml b/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml
index 5f58ee1a..8dd3d90c 100644
--- a/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml
+++ b/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml
@@ -1,38 +1,20 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Load Mesh as Red Point Cloud">
   <!--//////////-->
-  <BehaviorTree
-    ID="Load Mesh as Red Point Cloud"
-    _description="Loads a mesh from a file and colors it red."
-    _favorite="false"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Load Mesh as Red Point Cloud" _description="Loads a mesh from a file and colors it red." _favorite="false" _subtreeOnly="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="LoadPointCloudFromFile"
-        color="255;0;0"
-        file_path="~/user_ws/install/lab_sim/share/lab_sim/description/assets/Cube.stl"
-        point_cloud="{point_cloud}"
-      />
-      <Action
-        ID="TransformPointCloudFrame"
-        input_cloud="{point_cloud}"
-        output_cloud="{point_cloud}"
-      />
-      <Action
-        ID="TransformPointCloud"
-        input_cloud="{point_cloud}"
-        transform_pose="{initial_pose}"
-      />
+      <Action ID="LoadPointCloudFromFile" color="255;0;0" file_path="~/user_ws/install/lab_sim/share/lab_sim/description/assets/Cube.stl" point_cloud="{point_cloud}"/>
+      <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" output_cloud="{point_cloud}"/>
+      <Action ID="TransformPointCloud" input_cloud="{point_cloud}" transform_pose="{initial_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Load Mesh as Red Point Cloud">
-      <output_port name="point_cloud" default="{red_cloud}" />
-      <input_port name="_collapsed" default="false" />
-      <input_port name="initial_pose" default="{stamped_pose}" />
+      <output_port name="point_cloud" default="{red_cloud}"/>
+      <input_port name="_collapsed" default="false"/>
+      <input_port name="initial_pose" default="{stamped_pose}"/>
       <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
+        <Metadata subcategory="Perception - 3D Point Cloud"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/look_at_table.xml b/src/lab_sim/objectives/look_at_table.xml
index e7d6a131..4f4868e3 100644
--- a/src/lab_sim/objectives/look_at_table.xml
+++ b/src/lab_sim/objectives/look_at_table.xml
@@ -1,26 +1,15 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Look at table">
   <!--//////////-->
-  <BehaviorTree
-    ID="Look at table"
-    _description="Move the arm to a position that faces the table surface"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Look at table" _description="Move the arm to a position that faces the table surface" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        waypoint_name="Look at Table"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        use_all_planners="false"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Look at table">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/loop_detect_apriltag.xml b/src/lab_sim/objectives/loop_detect_apriltag.xml
index 9d5fe9c5..f8d627c5 100644
--- a/src/lab_sim/objectives/loop_detect_apriltag.xml
+++ b/src/lab_sim/objectives/loop_detect_apriltag.xml
@@ -1,39 +1,15 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Loop Detect AprilTag">
   <!--//////////-->
-  <BehaviorTree
-    ID="Loop Detect AprilTag"
-    _description="Loops finding April Tag markers."
-    _favorite="false"
-  >
+  <BehaviorTree ID="Loop Detect AprilTag" _description="Loops finding April Tag markers." _favorite="false">
     <Control ID="Sequence">
       <!--Keep executing until failure-->
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence" name="GetDetection">
-          <Action
-            ID="LoadObjectiveParameters"
-            config_file_name="51mm_apriltag_detection_config.yaml"
-            parameters="{parameters}"
-          />
-          <Action
-            ID="GetCameraInfo"
-            topic_name="/wrist_camera/camera_info"
-            message_out="{camera_info}"
-            timeout_sec="5.000000"
-          />
-          <Action
-            ID="GetImage"
-            topic_name="/wrist_camera/color"
-            message_out="{image}"
-            timeout_sec="5.000000"
-          />
-          <Action
-            ID="DetectAprilTags"
-            image="{image}"
-            camera_info="{camera_info}"
-            parameters="{parameters}"
-            detections="{detections}"
-          />
+          <Action ID="LoadObjectiveParameters" config_file_name="51mm_apriltag_detection_config.yaml" parameters="{parameters}"/>
+          <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" message_out="{camera_info}" timeout_sec="5.000000"/>
+          <Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}" timeout_sec="5.000000"/>
+          <Action ID="DetectAprilTags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/>
         </Control>
       </Decorator>
     </Control>
@@ -41,8 +17,8 @@
   <TreeNodesModel>
     <SubTree ID="Loop Detect AprilTag">
       <MetadataFields>
-        <Metadata subcategory="Perception - 2D Image" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Perception - 2D Image"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/move_along_square.xml b/src/lab_sim/objectives/move_along_square.xml
index af03683b..e7941e99 100644
--- a/src/lab_sim/objectives/move_along_square.xml
+++ b/src/lab_sim/objectives/move_along_square.xml
@@ -1,95 +1,23 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move Along Square">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Move Along Square"
-    _subtreeOnly="true"
-    _description="Move along a square path"
-  >
+  <BehaviorTree ID="Move Along Square" _subtreeOnly="true" _description="Move along a square path">
     <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        controller_names="/joint_trajectory_controller"
-        task="{mtc_task}"
-        task_id="move_along_axis"
-      />
-      <Action
-        ID="SetupMTCCurrentState"
-        skip_collision_check="false"
-        task="{mtc_task}"
-      />
-      <Action
-        ID="SetupMTCMoveAlongFrameAxis"
-        acceleration_scale="1.0"
-        axis_frame="world"
-        axis_y="0.0"
-        axis_z="1.0"
-        hand_frame="tool0"
-        ignore_environment_collisions="false"
-        max_distance="0.2"
-        min_distance="0.2"
-        planning_group_name="manipulator"
-        task="{mtc_task}"
-        velocity_scale="1.0"
-        axis_x="0.0"
-      />
-      <Action
-        ID="SetupMTCMoveAlongFrameAxis"
-        acceleration_scale="1.0"
-        axis_frame="world"
-        axis_y="0.0"
-        axis_z="0.0"
-        hand_frame="tool0"
-        ignore_environment_collisions="false"
-        max_distance="0.2"
-        min_distance="0.2"
-        planning_group_name="manipulator"
-        task="{mtc_task}"
-        velocity_scale="1.0"
-        axis_x="1.0"
-      />
-      <Action
-        ID="SetupMTCMoveAlongFrameAxis"
-        acceleration_scale="1.0"
-        axis_frame="world"
-        axis_y="0.0"
-        axis_z="-1.0"
-        hand_frame="tool0"
-        ignore_environment_collisions="false"
-        max_distance="0.2"
-        min_distance="0.2"
-        planning_group_name="manipulator"
-        task="{mtc_task}"
-        velocity_scale="1.0"
-        axis_x="0.0"
-      />
-      <Action
-        ID="SetupMTCMoveAlongFrameAxis"
-        acceleration_scale="1.0"
-        axis_frame="world"
-        axis_y="0.0"
-        axis_z="0.0"
-        hand_frame="tool0"
-        ignore_environment_collisions="false"
-        max_distance="0.2"
-        min_distance="0.2"
-        planning_group_name="manipulator"
-        task="{mtc_task}"
-        velocity_scale="1.0"
-        axis_x="-1.0"
-      />
-      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
-      <Action
-        ID="ExecuteMTCTask"
-        goal_duration_tolerance="-1.000000"
-        solution="{mtc_solution}"
-      />
+      <Action ID="InitializeMTCTask" controller_names="/joint_trajectory_controller" task="{mtc_task}" task_id="move_along_axis"/>
+      <Action ID="SetupMTCCurrentState" skip_collision_check="false" task="{mtc_task}"/>
+      <Action ID="SetupMTCMoveAlongFrameAxis" acceleration_scale="1.0" axis_frame="world" axis_y="0.0" axis_z="1.0" hand_frame="tool0" ignore_environment_collisions="false" max_distance="0.2" min_distance="0.2" planning_group_name="manipulator" task="{mtc_task}" velocity_scale="1.0" axis_x="0.0"/>
+      <Action ID="SetupMTCMoveAlongFrameAxis" acceleration_scale="1.0" axis_frame="world" axis_y="0.0" axis_z="0.0" hand_frame="tool0" ignore_environment_collisions="false" max_distance="0.2" min_distance="0.2" planning_group_name="manipulator" task="{mtc_task}" velocity_scale="1.0" axis_x="1.0"/>
+      <Action ID="SetupMTCMoveAlongFrameAxis" acceleration_scale="1.0" axis_frame="world" axis_y="0.0" axis_z="-1.0" hand_frame="tool0" ignore_environment_collisions="false" max_distance="0.2" min_distance="0.2" planning_group_name="manipulator" task="{mtc_task}" velocity_scale="1.0" axis_x="0.0"/>
+      <Action ID="SetupMTCMoveAlongFrameAxis" acceleration_scale="1.0" axis_frame="world" axis_y="0.0" axis_z="0.0" hand_frame="tool0" ignore_environment_collisions="false" max_distance="0.2" min_distance="0.2" planning_group_name="manipulator" task="{mtc_task}" velocity_scale="1.0" axis_x="-1.0"/>
+      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/>
+      <Action ID="ExecuteMTCTask" goal_duration_tolerance="-1.000000" solution="{mtc_solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move Along Square">
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/move_with_velocity_and_force.xml b/src/lab_sim/objectives/move_with_velocity_and_force.xml
index a39e2175..dcc28c67 100644
--- a/src/lab_sim/objectives/move_with_velocity_and_force.xml
+++ b/src/lab_sim/objectives/move_with_velocity_and_force.xml
@@ -1,91 +1,29 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Push Button">
   <!--//////////-->
-  <BehaviorTree
-    ID="Push Button"
-    _description="An example for how to use the VFC to move with a velocity / force reference until it collides"
-    _favorite="true"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Push Button" _description="An example for how to use the VFC to move with a velocity / force reference until it collides" _favorite="true" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Close Gripper" _collapsed="true" />
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        waypoint_name="VFC Start"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller"
-      />
-      <Action
-        ID="ActivateControllers"
-        controller_names="/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}"
-      />
+      <SubTree ID="Close Gripper" _collapsed="true"/>
+      <SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="VFC Start" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
+      <Action ID="ActivateControllers" controller_names="/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="20"
-        />
-        <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"
-        />
+        <Action ID="ForceExceedsThreshold" minimum_consecutive_wrench_values="5" wrench_frame_name="tool0" hand_frame_name="grasp_link" force_threshold="20"/>
+        <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"/>
       </Control>
       <!--Relax the arm for 500ms to just stay in contact without applying force.-->
-      <SubTree
-        ID="Force Relaxation"
-        _collapsed="true"
-        wrench_gain="0.01"
-        relaxation_time_ms="500"
-        tip_link="grasp_link"
-      />
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        joint_group_name="manipulator"
-        planner_interface="moveit_default"
-        controller_names="/joint_trajectory_controller"
-        waypoint_name="Look at Table"
-      />
+      <SubTree ID="Force Relaxation" _collapsed="true" wrench_gain="0.01" relaxation_time_ms="500" tip_link="grasp_link"/>
+      <SubTree ID="Move to Waypoint" _collapsed="true" joint_group_name="manipulator" planner_interface="moveit_default" controller_names="/joint_trajectory_controller" waypoint_name="Look at Table"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Push Button">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/open_gripper.xml b/src/lab_sim/objectives/open_gripper.xml
index 14921bf6..aada5976 100644
--- a/src/lab_sim/objectives/open_gripper.xml
+++ b/src/lab_sim/objectives/open_gripper.xml
@@ -1,23 +1,15 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Open Gripper">
   <!--//////////-->
-  <BehaviorTree
-    ID="Open Gripper"
-    _description="Open the gripper"
-    _favorite="false"
-  >
+  <BehaviorTree ID="Open Gripper" _description="Open the gripper" _favorite="false">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.05"
-        timeout="15.000000"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.05" timeout="15.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Open Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/pick_and_place_example.xml b/src/lab_sim/objectives/pick_and_place_example.xml
index 5669d25f..2243679f 100644
--- a/src/lab_sim/objectives/pick_and_place_example.xml
+++ b/src/lab_sim/objectives/pick_and_place_example.xml
@@ -1,92 +1,33 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick And Place Example">
   <!--//////////-->
-  <BehaviorTree
-    ID="Pick And Place Example"
-    _description="Picks up a cube and then places it."
-    _favorite="false"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Pick And Place Example" _description="Picks up a cube and then places it." _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="Pick and Place">
-      <SubTree ID="Open Gripper" />
+      <SubTree ID="Open Gripper"/>
       <Control ID="Sequence" name="Pick">
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="world"
-          position_xyz="0.01;.75;0.515"
-          orientation_xyzw="0;1;0;0"
-          stamped_pose="{pick_pose}"
-        />
-        <Action
-          ID="InitializeMTCTask"
-          task_id="pick_task"
-          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          task="{mtc_pick_task}"
-        />
-        <Action
-          ID="SetupMTCCurrentState"
-          task="{mtc_pick_task}"
-          skip_collision_check="false"
-        />
+        <Action ID="CreateStampedPose" reference_frame="world" position_xyz="0.01;.75;0.515" orientation_xyzw="0;1;0;0" stamped_pose="{pick_pose}"/>
+        <Action ID="InitializeMTCTask" task_id="pick_task" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{mtc_pick_task}"/>
+        <Action ID="SetupMTCCurrentState" task="{mtc_pick_task}" skip_collision_check="false"/>
         <!--The following behavior can be found in the example_behaviors package.-->
-        <Action
-          ID="SetupMtcPickFromPose"
-          grasp_pose="{pick_pose}"
-          task="{mtc_pick_task}"
-        />
-        <Action
-          ID="PlanMTCTask"
-          task="{mtc_pick_task}"
-          solution="{mtc_pick_solution}"
-        />
-        <Action
-          ID="ExecuteMTCTask"
-          solution="{mtc_pick_solution}"
-          goal_duration_tolerance="-1.000000"
-        />
+        <Action ID="SetupMtcPickFromPose" grasp_pose="{pick_pose}" task="{mtc_pick_task}"/>
+        <Action ID="PlanMTCTask" task="{mtc_pick_task}" solution="{mtc_pick_solution}"/>
+        <Action ID="ExecuteMTCTask" solution="{mtc_pick_solution}" goal_duration_tolerance="-1.000000"/>
       </Control>
       <Control ID="Sequence" name="Place">
-        <Action
-          ID="InitializeMTCTask"
-          task_id="place_task"
-          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          task="{mtc_place_task}"
-        />
-        <Action
-          ID="SetupMTCCurrentState"
-          task="{mtc_place_task}"
-          skip_collision_check="false"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="world"
-          position_xyz="0.01;.75;0.5.15"
-          orientation_xyzw="0;1;0;0"
-          stamped_pose="{place_pose}"
-        />
+        <Action ID="InitializeMTCTask" task_id="place_task" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{mtc_place_task}"/>
+        <Action ID="SetupMTCCurrentState" task="{mtc_place_task}" skip_collision_check="false"/>
+        <Action ID="CreateStampedPose" reference_frame="world" position_xyz="0.01;.75;0.5.15" orientation_xyzw="0;1;0;0" stamped_pose="{place_pose}"/>
         <!--The following behavior can be found in the example_behaviors package.-->
-        <Action
-          ID="SetupMtcPlaceFromPose"
-          place_pose="{place_pose}"
-          task="{mtc_place_task}"
-        />
-        <Action
-          ID="PlanMTCTask"
-          task="{mtc_place_task}"
-          solution="{mtc_place_solution}"
-        />
-        <Action
-          ID="ExecuteMTCTask"
-          solution="{mtc_place_solution}"
-          goal_duration_tolerance="-1.000000"
-        />
+        <Action ID="SetupMtcPlaceFromPose" place_pose="{place_pose}" task="{mtc_place_task}"/>
+        <Action ID="PlanMTCTask" task="{mtc_place_task}" solution="{mtc_place_solution}"/>
+        <Action ID="ExecuteMTCTask" solution="{mtc_place_solution}" goal_duration_tolerance="-1.000000"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Pick And Place Example">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
index 51e17012..4c329b85 100644
--- a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
+++ b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
@@ -1,122 +1,37 @@
-<?xml version="1.0" encoding="utf-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Pick April Tag Labeled Object With Approval"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object With Approval">
   <!--//////////-->
-  <BehaviorTree
-    ID="Pick April Tag Labeled Object With Approval"
-    _description="Picks up an object that has an AprilTag marker."
-    _favorite="false"
-  >
+  <BehaviorTree ID="Pick April Tag Labeled Object With Approval" _description="Picks up an object that has an AprilTag marker." _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Look at table" _collapsed="true" />
-      <Action ID="ClearSnapshot" />
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Look at table" _collapsed="true"/>
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
       <Control ID="Sequence" name="Setup">
-        <SubTree ID="Open Gripper" />
+        <SubTree ID="Open Gripper"/>
       </Control>
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="apriltag_detection_config.yaml"
-        parameters="{parameters}"
-      />
-      <Action
-        ID="GetCameraInfo"
-        topic_name="/wrist_camera/camera_info"
-        message_out="{camera_info}"
-        timeout_sec="5.000000"
-      />
-      <Action
-        ID="GetImage"
-        topic_name="/wrist_camera/color"
-        message_out="{image}"
-        timeout_sec="5.000000"
-      />
-      <SubTree
-        ID="Get AprilTag Pose from Image"
-        image="{image}"
-        camera_info="{camera_info}"
-        parameters="{parameters}"
-        _collapsed="true"
-        detection_pose="{detection_pose}"
-      />
-      <Action
-        ID="TransformPose"
-        output_pose="{view_pose}"
-        input_pose="{detection_pose}"
-        quaternion_xyzw="0;0;0;1"
-        translation_xyz="0;-0.1;-0.15"
-      />
-      <SubTree
-        ID="Plan Move To Pose"
-        target_pose="{view_pose}"
-        move_to_pose_solution="{move_to_pose_solution}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{move_to_pose_solution}"
-        goal_duration_tolerance="-1.000000"
-      />
-      <SubTree
-        ID="Get AprilTag Pose from Image"
-        image="{image}"
-        camera_info="{camera_info}"
-        parameters="{parameters}"
-        _collapsed="true"
-        detection_pose="{detection_pose}"
-      />
-      <Action
-        ID="TransformPose"
-        output_pose="{offset_pose}"
-        input_pose="{detection_pose}"
-        quaternion_xyzw="0;0;0;1"
-        translation_xyz="0;0;-0.01"
-      />
-      <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}"
-        skip_collision_check="false"
-      />
-      <Action
-        ID="SetupMTCPickObject"
-        grasp_pose="{offset_pose}"
-        task="{pick_object_task}"
-        parameters="{parameters}"
-        monitored_stage="current state"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{pick_object_solution}"
-        task="{pick_object_task}"
-      />
-      <Action
-        ID="WaitForUserTrajectoryApproval"
-        solution="{pick_object_solution}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{pick_object_solution}"
-        goal_duration_tolerance="-1.000000"
-      />
+      <Action ID="LoadObjectiveParameters" config_file_name="apriltag_detection_config.yaml" parameters="{parameters}"/>
+      <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" message_out="{camera_info}" timeout_sec="5.000000"/>
+      <Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}" timeout_sec="5.000000"/>
+      <SubTree ID="Get AprilTag Pose from Image" image="{image}" camera_info="{camera_info}" parameters="{parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
+      <Action ID="TransformPose" output_pose="{view_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;-0.1;-0.15"/>
+      <SubTree ID="Plan Move To Pose" target_pose="{view_pose}" move_to_pose_solution="{move_to_pose_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" goal_duration_tolerance="-1.000000"/>
+      <SubTree ID="Get AprilTag Pose from Image" image="{image}" camera_info="{camera_info}" parameters="{parameters}" _collapsed="true" detection_pose="{detection_pose}"/>
+      <Action ID="TransformPose" output_pose="{offset_pose}" input_pose="{detection_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;-0.01"/>
+      <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}" skip_collision_check="false"/>
+      <Action ID="SetupMTCPickObject" grasp_pose="{offset_pose}" task="{pick_object_task}" parameters="{parameters}" monitored_stage="current state"/>
+      <Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
+      <Action ID="WaitForUserTrajectoryApproval" solution="{pick_object_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" goal_duration_tolerance="-1.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Pick April Tag Labeled Object With Approval">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/pick_from_pose.xml b/src/lab_sim/objectives/pick_from_pose.xml
index 04af04b8..b1469202 100644
--- a/src/lab_sim/objectives/pick_from_pose.xml
+++ b/src/lab_sim/objectives/pick_from_pose.xml
@@ -1,48 +1,21 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick from Pose">
   <!--//////////-->
-  <BehaviorTree
-    ID="Pick from Pose"
-    _description="Takes a grasp pose as input and picks up the object at that pose"
-    _favorite="false"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Pick from Pose" _description="Takes a grasp pose as input and picks up the object at that pose" _favorite="false" _subtreeOnly="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="LoadObjectiveParameters"
-        parameters="{parameters}"
-        config_file_name="pick_object_config.yaml"
-      />
-      <Action
-        ID="InitializeMTCTask"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        task="{mtc_task}"
-        task_id="pick_object"
-      />
-      <Action
-        ID="SetupMTCCurrentState"
-        skip_collision_check="false"
-        task="{mtc_task}"
-      />
-      <Action
-        ID="SetupMTCPickObject"
-        grasp_pose="{grasp_pose}"
-        monitored_stage="current state"
-        task="{mtc_task}"
-        parameters="{parameters}"
-      />
-      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
-      <Action
-        ID="ExecuteMTCTask"
-        goal_duration_tolerance="-1.000000"
-        solution="{mtc_solution}"
-      />
+      <Action ID="LoadObjectiveParameters" parameters="{parameters}" config_file_name="pick_object_config.yaml"/>
+      <Action ID="InitializeMTCTask" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{mtc_task}" task_id="pick_object"/>
+      <Action ID="SetupMTCCurrentState" skip_collision_check="false" task="{mtc_task}"/>
+      <Action ID="SetupMTCPickObject" grasp_pose="{grasp_pose}" monitored_stage="current state" task="{mtc_task}" parameters="{parameters}"/>
+      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/>
+      <Action ID="ExecuteMTCTask" goal_duration_tolerance="-1.000000" solution="{mtc_solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Pick from Pose">
-      <inout_port name="grasp_pose" default="{object_pose}" />
+      <inout_port name="grasp_pose" default="{object_pose}"/>
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/pick_object.xml b/src/lab_sim/objectives/pick_object.xml
index 0f0ec4c8..0b272445 100644
--- a/src/lab_sim/objectives/pick_object.xml
+++ b/src/lab_sim/objectives/pick_object.xml
@@ -1,39 +1,20 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick object">
   <!--//////////-->
-  <BehaviorTree
-    ID="Pick object"
-    _description="Opens the gripper, defines a target pose for the object and performs a motion for picking it."
-    _favorite="false"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Pick object" _description="Opens the gripper, defines a target pose for the object and performs a motion for picking it." _favorite="false" _subtreeOnly="true">
     <Control ID="Sequence">
-      <SubTree ID="Open Gripper" />
-      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;1;0;0"
-        position_xyz="0.01;.75;0.515"
-        stamped_pose="{stamped_pose}"
-        reference_frame="world"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{stamped_pose}"
-        vector="{pose_vector}"
-      />
-      <SubTree
-        ID="Pick from Pose"
-        _collapsed="true"
-        grasp_pose="{stamped_pose}"
-      />
+      <SubTree ID="Open Gripper"/>
+      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;1;0;0" position_xyz="0.01;.75;0.515" stamped_pose="{stamped_pose}" reference_frame="world"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_vector}"/>
+      <SubTree ID="Pick from Pose" _collapsed="true" grasp_pose="{stamped_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Pick object">
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
-        <Metadata runnable="false" />
+        <Metadata subcategory="Motion - Execute"/>
+        <Metadata runnable="false"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/pick_up_cube.xml b/src/lab_sim/objectives/pick_up_cube.xml
index 6b46cfc3..dccf19c8 100644
--- a/src/lab_sim/objectives/pick_up_cube.xml
+++ b/src/lab_sim/objectives/pick_up_cube.xml
@@ -1,23 +1,19 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick up Cube">
   <!--//////////-->
-  <BehaviorTree
-    ID="Pick up Cube"
-    _description="Pick up a cube from a manually defined location"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Pick up Cube" _description="Pick up a cube from a manually defined location" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Clear Snapshot" _collapsed="true" />
-      <SubTree ID="Look at table" _collapsed="true" />
+      <SubTree ID="Clear Snapshot" _collapsed="true"/>
+      <SubTree ID="Look at table" _collapsed="true"/>
       <!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
-      <SubTree ID="Pick object" _collapsed="false" />
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
+      <SubTree ID="Pick object" _collapsed="false"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Pick up Cube">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/place_at_pose.xml b/src/lab_sim/objectives/place_at_pose.xml
index 76c3bb36..1865e0c9 100644
--- a/src/lab_sim/objectives/place_at_pose.xml
+++ b/src/lab_sim/objectives/place_at_pose.xml
@@ -1,56 +1,23 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Place at Pose">
   <!--//////////-->
-  <BehaviorTree
-    ID="Place at Pose"
-    _subtreeOnly="true"
-    _description="Places object in gripper at the given input pose"
-  >
+  <BehaviorTree ID="Place at Pose" _subtreeOnly="true" _description="Places object in gripper at the given input pose">
     <Control ID="Sequence">
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="place_object_config.yaml"
-        parameters="{parameters}"
-      />
-      <Action
-        ID="InitializeMTCTask"
-        task_id="place_object"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        task="{place_object_task}"
-      />
-      <Action
-        ID="SetupMTCCurrentState"
-        task="{place_object_task}"
-        skip_collision_check="false"
-      />
-      <Action
-        ID="SetupMTCPickObject"
-        grasp_pose="{grasp_pose}"
-        task="{place_object_task}"
-        parameters="{parameters}"
-        monitored_stage="current state"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{place_object_solution}"
-        task="{place_object_task}"
-      />
-      <SubTree
-        ID="Wait for Trajectory Approval if User Available"
-        solution="{place_object_solution}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{place_object_solution}"
-        goal_duration_tolerance="-1.000000"
-      />
+      <Action ID="LoadObjectiveParameters" config_file_name="place_object_config.yaml" parameters="{parameters}"/>
+      <Action ID="InitializeMTCTask" task_id="place_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{place_object_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{place_object_task}" skip_collision_check="false"/>
+      <Action ID="SetupMTCPickObject" grasp_pose="{grasp_pose}" task="{place_object_task}" parameters="{parameters}" monitored_stage="current state"/>
+      <Action ID="PlanMTCTask" solution="{place_object_solution}" task="{place_object_task}"/>
+      <SubTree ID="Wait for Trajectory Approval if User Available" solution="{place_object_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{place_object_solution}" goal_duration_tolerance="-1.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Place at pose">
-      <inout_port name="grasp_pose" default="{object_pose}" />
+      <inout_port name="grasp_pose" default="{object_pose}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
-        <Metadata runnable="false" />
+        <Metadata subcategory="Motion - Execute"/>
+        <Metadata runnable="false"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/place_object.xml b/src/lab_sim/objectives/place_object.xml
index 12399230..678aeea4 100644
--- a/src/lab_sim/objectives/place_object.xml
+++ b/src/lab_sim/objectives/place_object.xml
@@ -1,35 +1,20 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Place Object">
   <!--//////////-->
-  <BehaviorTree
-    ID="Place Object"
-    _description="Place an object in the gripper at a manually specified location"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Place Object" _description="Place an object in the gripper at a manually specified location" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="root">
       <Control ID="Sequence">
-        <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
-        <Action
-          ID="CreateStampedPose"
-          orientation_xyzw="0;1;0;0"
-          position_xyz="0.01;.75;0.5.15"
-          stamped_pose="{stamped_pose}"
-          reference_frame="world"
-        />
+        <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
+        <Action ID="CreateStampedPose" orientation_xyzw="0;1;0;0" position_xyz="0.01;.75;0.5.15" stamped_pose="{stamped_pose}" reference_frame="world"/>
       </Control>
-      <SubTree
-        ID="Place at Pose"
-        _collapsed="true"
-        grasp_pose="{stamped_pose}"
-      />
+      <SubTree ID="Place at Pose" _collapsed="true" grasp_pose="{stamped_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Place Object">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Basic Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/plan_and_save_trajectory.xml b/src/lab_sim/objectives/plan_and_save_trajectory.xml
index 567f0076..f1a637e5 100644
--- a/src/lab_sim/objectives/plan_and_save_trajectory.xml
+++ b/src/lab_sim/objectives/plan_and_save_trajectory.xml
@@ -1,64 +1,24 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Plan and Save Trajectory">
   <!--//////////-->
-  <BehaviorTree
-    ID="Plan and Save Trajectory"
-    _description="Use a series of waypoints to plan and MTC task, convert the solution to a Joint Trajectory message, and save it to disk in YAML format"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Plan and Save Trajectory" _description="Use a series of waypoints to plan and MTC task, convert the solution to a Joint Trajectory message, and save it to disk in YAML format" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <!--Before we plan we want to make sure we are at a known position, so when we execute the plan it can be done from the same position-->
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        waypoint_name="Workspace Right"
-        joint_group_name="manipulator"
-        planner_interface="moveit_default"
-        controller_names="/joint_trajectory_controller"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Workspace Right" joint_group_name="manipulator" planner_interface="moveit_default" controller_names="/joint_trajectory_controller" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0"/>
       <!--Create a vector of poses we want to move through-->
-      <SubTree
-        ID="Create Pose Vector"
-        _collapsed="false"
-        target_poses="{target_poses}"
-      />
+      <SubTree ID="Create Pose Vector" _collapsed="false" target_poses="{target_poses}"/>
       <!--Plan a cartesian path through the desired poses-->
-      <Action
-        ID="PlanCartesianPath"
-        acceleration_scale_factor="1.000000"
-        blending_radius="0.020000"
-        ik_cartesian_space_density="0.010000"
-        ik_joint_space_density="0.100000"
-        path="{target_poses}"
-        planning_group_name="manipulator"
-        position_only="false"
-        tip_link="grasp_link"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="1.000000"
-        debug_solution="{debug_solution}"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <Action ID="PlanCartesianPath" acceleration_scale_factor="1.000000" blending_radius="0.020000" ik_cartesian_space_density="0.010000" ik_joint_space_density="0.100000" path="{target_poses}" planning_group_name="manipulator" position_only="false" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="1.000000" debug_solution="{debug_solution}" joint_trajectory_msg="{joint_trajectory_msg}"/>
       <!--Preview the trajectory for approval and save if approved-->
-      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
-      <Action
-        ID="SaveJointTrajectoryToYaml"
-        yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"
-        message="{joint_trajectory_msg}"
-      />
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      <Action ID="SaveJointTrajectoryToYaml" yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory" message="{joint_trajectory_msg}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Plan and Save Trajectory">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Basic Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/plan_move_to_pose.xml b/src/lab_sim/objectives/plan_move_to_pose.xml
index 1ecd691b..ce9d3a7b 100644
--- a/src/lab_sim/objectives/plan_move_to_pose.xml
+++ b/src/lab_sim/objectives/plan_move_to_pose.xml
@@ -1,42 +1,20 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Plan Move To Pose">
   <!--//////////-->
-  <BehaviorTree
-    ID="Plan Move To Pose"
-    _subtreeOnly="true"
-    _description="Plan an MTC task, but do not execute it"
-  >
+  <BehaviorTree ID="Plan Move To Pose" _subtreeOnly="true" _description="Plan an MTC task, but do not execute it">
     <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_pose"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        task="{move_to_pose_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
-      <Action
-        ID="SetupMTCPlanToPose"
-        ik_frame="grasp_link"
-        planning_group_name="manipulator"
-        target_pose="{target_pose}"
-        task="{move_to_pose_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_pose_solution}"
-        task="{move_to_pose_task}"
-      />
+      <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCPlanToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}"/>
+      <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Plan Move To Pose">
-      <input_port name="target_pose" default="{target_pose}" />
-      <output_port
-        name="move_to_pose_solution"
-        default="{move_to_pose_solution}"
-      />
+      <input_port name="target_pose" default="{target_pose}"/>
+      <output_port name="move_to_pose_solution" default="{move_to_pose_solution}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Planning" />
+        <Metadata subcategory="Motion - Planning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/plan_to_pose.xml b/src/lab_sim/objectives/plan_to_pose.xml
index 23090261..4dcdd7bd 100644
--- a/src/lab_sim/objectives/plan_to_pose.xml
+++ b/src/lab_sim/objectives/plan_to_pose.xml
@@ -1,58 +1,32 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Add Poses to MTC Task">
   <!--//////////-->
-  <BehaviorTree
-    ID="Add Poses to MTC Task"
-    _description="Initializes a new MTC task with the current state and adds a sequence of target poses."
-    _favorite="false"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Add Poses to MTC Task" _description="Initializes a new MTC task with the current state and adds a sequence of target poses." _favorite="false" _subtreeOnly="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="InitializeMTCTask"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        task="{mtc_task}"
-      />
+      <Action ID="InitializeMTCTask" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{mtc_task}"/>
       <!-- Start the MTC task from the robot's current pose -->
-      <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
+      <Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
       <!-- Use a script with a pose counter so we can give a unique name to each marker (allowing multiple poses to be visualized) -->
-      <Action ID="Script" code="pose_ctr := 0" />
-      <Decorator
-        ID="ForEach"
-        vector_in="{target_poses}"
-        out="{target_pose}"
-        index="{index}"
-      >
+      <Action ID="Script" code="pose_ctr := 0"/>
+      <Decorator ID="ForEach" vector_in="{target_poses}" out="{target_pose}" index="{index}">
         <Control ID="Sequence">
           <!-- Visualize and add each pose to the vector -->
-          <Action
-            ID="VisualizePose"
-            marker_name="{pose_ctr}"
-            pose="{target_pose}"
-          />
-          <Action ID="Script" code="pose_ctr += 1" />
-          <Action
-            ID="SetupMTCPlanToPose"
-            ik_frame="{ik_frame}"
-            planning_group_name="{planning_group_name}"
-            target_pose="{target_pose}"
-            task="{mtc_task}"
-          />
+          <Action ID="VisualizePose" marker_name="{pose_ctr}" pose="{target_pose}"/>
+          <Action ID="Script" code="pose_ctr += 1"/>
+          <Action ID="SetupMTCPlanToPose" ik_frame="{ik_frame}" planning_group_name="{planning_group_name}" target_pose="{target_pose}" task="{mtc_task}"/>
         </Control>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Add Poses to MTC Task">
-      <input_port name="target_poses" default="{target_poses}" />
-      <input_port
-        name="controller_names"
-        default="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
-      <input_port name="planning_group_name" default="manipulator" />
-      <input_port name="ik_frame" default="grasp_link" />
-      <inout_port name="mtc_task" default="{mtc_task}" />
+      <input_port name="target_poses" default="{target_poses}"/>
+      <input_port name="controller_names" default="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <input_port name="planning_group_name" default="manipulator"/>
+      <input_port name="ik_frame" default="grasp_link"/>
+      <inout_port name="mtc_task" default="{mtc_task}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Task Planning" />
+        <Metadata subcategory="Motion - Task Planning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/playback_square_trajectory.xml b/src/lab_sim/objectives/playback_square_trajectory.xml
index 3421950b..eb1bdeb5 100644
--- a/src/lab_sim/objectives/playback_square_trajectory.xml
+++ b/src/lab_sim/objectives/playback_square_trajectory.xml
@@ -1,52 +1,19 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Playback Square Trajectory">
   <!--//////////-->
-  <BehaviorTree
-    ID="Playback Square Trajectory"
-    _description="Loads and plays a JointTrajectory of the end-effector moving in a square trajectory."
-    _favorite="true"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Playback Square Trajectory" _description="Loads and plays a JointTrajectory of the end-effector moving in a square trajectory." _favorite="true" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="LoadJointTrajectoryFromYaml"
-        file_path="square_traj.yaml"
-        output="{joint_trajectory_msg}"
-      />
-      <Action
-        ID="GetTrajectoryStateAtTime"
-        from_start="true"
-        joint_state="{joint_state}"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-        time_from_reference="0.000000"
-      />
-      <SubTree
-        ID="Move to Joint State"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        target_joint_state="{joint_state}"
-        velocity_scale_factor="1.0"
-      />
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-        goal_duration_tolerance="-1.000000"
-        goal_position_tolerance="0.000000"
-        goal_time_tolerance="0.000000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <Action ID="LoadJointTrajectoryFromYaml" file_path="square_traj.yaml" output="{joint_trajectory_msg}"/>
+      <Action ID="GetTrajectoryStateAtTime" from_start="true" joint_state="{joint_state}" joint_trajectory_msg="{joint_trajectory_msg}" time_from_reference="0.000000"/>
+      <SubTree ID="Move to Joint State" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" target_joint_state="{joint_state}" velocity_scale_factor="1.0"/>
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Playback Square Trajectory">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/push_button_with_a_trajectory.xml b/src/lab_sim/objectives/push_button_with_a_trajectory.xml
index ea7c6663..1cb0ffa6 100644
--- a/src/lab_sim/objectives/push_button_with_a_trajectory.xml
+++ b/src/lab_sim/objectives/push_button_with_a_trajectory.xml
@@ -1,94 +1,28 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Push Button With a Trajectory">
   <!--//////////-->
-  <BehaviorTree
-    ID="Push Button With a Trajectory"
-    _description="An example for how to use the JTAC to execute a trajectory with compliance."
-    _favorite="true"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Push Button With a Trajectory" _description="An example for how to use the JTAC to execute a trajectory with compliance." _favorite="true" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Close Gripper" _collapsed="true" />
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        waypoint_name="VFC Start"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        keep_orientation="false"
-        keep_orientation_link_names="grasp_link"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-      />
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Close Gripper" _collapsed="true"/>
+      <SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="VFC Start" joint_group_name="manipulator" controller_names="/joint_trajectory_controller" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0"/>
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
       <!--Plan a path to move in straight line towards the button-->
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{target_pose}"
-        position_xyz="0;0;0.11"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="pose"
-        marker_size="0.100000"
-        pose="{target_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{target_pose}"
-        vector="{path}"
-      />
-      <Action
-        ID="PlanCartesianPath"
-        acceleration_scale_factor="0.200000"
-        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="{path}"
-        planning_group_name="manipulator"
-        position_only="true"
-        tip_link="grasp_link"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="0.1"
-      />
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{target_pose}" position_xyz="0;0;0.11"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{target_pose}"/>
+      <Action ID="AddPoseStampedToVector" input="{target_pose}" vector="{path}"/>
+      <Action ID="PlanCartesianPath" acceleration_scale_factor="0.200000" 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="{path}" planning_group_name="manipulator" position_only="true" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="0.1"/>
       <!--Activate the JTAC, tare the sensor and execute the trajectory with compliance-->
-      <Action
-        ID="ActivateControllers"
-        controller_names="joint_trajectory_admittance_controller"
-      />
-      <Action
-        ID="CallTriggerService"
-        service_name="/joint_trajectory_admittance_controller/zero_fts"
-      />
-      <Action
-        ID="SetAdmittanceParameters"
-        admittance_parameters_msg="{admittance_parameters_msg}"
-        config_file_name="push_button_admittance_params.yaml"
-      />
-      <Action
-        ID="ExecuteTrajectoryWithAdmittance"
-        absolute_force_torque_threshold="1000;1000;1000;1000;1000;1000"
-        admittance_parameters_msg="{admittance_parameters_msg}"
-        controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
-        goal_duration_tolerance="-1.000000"
-        goal_position_tolerance="0.001000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-        path_position_tolerance="0.20000"
-      />
+      <Action ID="ActivateControllers" controller_names="joint_trajectory_admittance_controller"/>
+      <Action ID="CallTriggerService" service_name="/joint_trajectory_admittance_controller/zero_fts"/>
+      <Action ID="SetAdmittanceParameters" admittance_parameters_msg="{admittance_parameters_msg}" config_file_name="push_button_admittance_params.yaml"/>
+      <Action ID="ExecuteTrajectoryWithAdmittance" absolute_force_torque_threshold="1000;1000;1000;1000;1000;1000" admittance_parameters_msg="{admittance_parameters_msg}" controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.001000" joint_trajectory_msg="{joint_trajectory_msg}" path_position_tolerance="0.20000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Push Button With a Trajectory">
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Motion - Execute"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/record_and_replay_scanning_motion.xml b/src/lab_sim/objectives/record_and_replay_scanning_motion.xml
index 0db9b710..52ab05f3 100644
--- a/src/lab_sim/objectives/record_and_replay_scanning_motion.xml
+++ b/src/lab_sim/objectives/record_and_replay_scanning_motion.xml
@@ -1,24 +1,19 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Record and Replay Scanning Motion">
   <!--//////////-->
-  <BehaviorTree
-    ID="Record and Replay Scanning Motion"
-    _description="Build a trajectory from a series of waypoints and MTC tasks to scan the environment, then save the trajectory to disk. If the trajectory is approved, it will be saved and then executed."
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Record and Replay Scanning Motion" _description="Build a trajectory from a series of waypoints and MTC tasks to scan the environment, then save the trajectory to disk. If the trajectory is approved, it will be saved and then executed." _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <!--First we build the trajectory using caresian and joint-state waypoint. If the trajecotry is approved, it will be saved.-->
-      <SubTree ID="Plan and Save Trajectory" _collapsed="false" />
+      <SubTree ID="Plan and Save Trajectory" _collapsed="false"/>
       <!--Next we load the saved trajectory from disk an execute it. This stage does not require motion planning-->
-      <SubTree ID="Load and Execute Joint Trajectory" _collapsed="false" />
+      <SubTree ID="Load and Execute Joint Trajectory" _collapsed="false"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Record and Replay Scanning Motion">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/record_square_trajectory.xml b/src/lab_sim/objectives/record_square_trajectory.xml
index 9f6e3d74..b854a18e 100644
--- a/src/lab_sim/objectives/record_square_trajectory.xml
+++ b/src/lab_sim/objectives/record_square_trajectory.xml
@@ -1,55 +1,18 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Record Square Trajectory">
   <!--//////////-->
-  <BehaviorTree
-    ID="Record Square Trajectory"
-    _description="Executes a square trajectory, records it, and saved the recording to a YAML file."
-    _favorite="true"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Record Square Trajectory" _description="Executes a square trajectory, records it, and saved the recording to a YAML file." _favorite="true" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <Control ID="Parallel" failure_count="1" success_count="1">
         <Decorator ID="KeepRunningUntilFailure">
-          <Action
-            ID="RecordJointTrajectory"
-            joint_state_topic="/joint_states"
-            joint_trajectory_message="{joint_trajectory_msg}"
-            planning_group_name="manipulator"
-            start_recording_service="/start_recording_joint_trajectory"
-            stop_recording_service="/stop_recording_joint_trajectory"
-            timeout_sec="5.000000"
-            joint_space_step="0.01"
-          />
+          <Action ID="RecordJointTrajectory" joint_state_topic="/joint_states" joint_trajectory_message="{joint_trajectory_msg}" planning_group_name="manipulator" start_recording_service="/start_recording_joint_trajectory" stop_recording_service="/stop_recording_joint_trajectory" timeout_sec="5.000000" joint_space_step="0.01"/>
         </Decorator>
         <Control ID="Sequence">
-          <SubTree
-            ID="Move to Waypoint"
-            _collapsed="true"
-            acceleration_scale_factor="1.0"
-            controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-            controller_names="/joint_trajectory_controller"
-            joint_group_name="manipulator"
-            keep_orientation="false"
-            keep_orientation_link_names="grasp_link"
-            keep_orientation_tolerance="0.05"
-            link_padding="0.01"
-            velocity_scale_factor="1.0"
-            waypoint_name="Beaker 3a - Pre Pick"
-          />
-          <Action
-            ID="CallTriggerService"
-            service_name="/start_recording_joint_trajectory"
-          />
-          <SubTree ID="Move Along Square" _collapsed="true" />
-          <Action
-            ID="CallTriggerService"
-            service_name="/stop_recording_joint_trajectory"
-          />
-          <Action
-            ID="SaveJointTrajectoryToYaml"
-            message="{joint_trajectory_msg}"
-            yaml_filename="~/user_ws/src/lab_sim/objectives/square_traj"
-          />
+          <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0" waypoint_name="Beaker 3a - Pre Pick"/>
+          <Action ID="CallTriggerService" service_name="/start_recording_joint_trajectory"/>
+          <SubTree ID="Move Along Square" _collapsed="true"/>
+          <Action ID="CallTriggerService" service_name="/stop_recording_joint_trajectory"/>
+          <Action ID="SaveJointTrajectoryToYaml" message="{joint_trajectory_msg}" yaml_filename="~/user_ws/src/lab_sim/objectives/square_traj"/>
         </Control>
       </Control>
     </Control>
@@ -57,8 +20,8 @@
   <TreeNodesModel>
     <SubTree ID="Record Square Trajectory">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/register_cad_part.xml b/src/lab_sim/objectives/register_cad_part.xml
index b9b0a2b5..c80fabe6 100644
--- a/src/lab_sim/objectives/register_cad_part.xml
+++ b/src/lab_sim/objectives/register_cad_part.xml
@@ -1,67 +1,26 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Register CAD Part">
   <!--//////////-->
-  <BehaviorTree
-    ID="Register CAD Part"
-    _description="Load an STL from disk and use registration to align the pointcloud with a matching shape in the environment"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Register CAD Part" _description="Load an STL from disk and use registration to align the pointcloud with a matching shape in the environment" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="ClearSnapshot" />
-      <SubTree ID="Look at table" _collapsed="true" />
-      <Action
-        ID="CreateStampedPose"
-        position_xyz="0.02;.75;0.58"
-        orientation_xyzw="0;0;0;1"
-        stamped_pose="{stamped_pose}"
-        name="Initial Guess"
-      />
-      <SubTree
-        ID="Load Mesh as Red Point Cloud"
-        _collapsed="false"
-        output_cloud="{red_cloud}"
-        initial_pose="{stamped_pose}"
-        point_cloud="{red_cloud}"
-      />
-      <Action ID="SendPointCloudToUI" point_cloud="{red_cloud}" />
-      <Action
-        ID="GetPointCloud"
-        topic_name="/wrist_camera/points"
-        message_out="{wrist_point_cloud}"
-      />
-      <Action
-        ID="TransformPointCloudFrame"
-        input_cloud="{wrist_point_cloud}"
-        output_cloud="{wrist_point_cloud}"
-      />
-      <Action ID="SendPointCloudToUI" point_cloud="{wrist_point_cloud}" />
-      <Action
-        ID="RegisterPointClouds"
-        target_point_cloud="{wrist_point_cloud}"
-        base_point_cloud="{red_cloud}"
-        max_correspondence_distance="0.5"
-        max_iterations="100"
-      />
-      <SubTree
-        ID="Load Mesh as Green Point Cloud"
-        _collapsed="false"
-        output_cloud="{green_cloud}"
-        initial_pose="{stamped_pose}"
-        point_cloud="{green_cloud}"
-      />
-      <Action
-        ID="TransformPointCloud"
-        input_cloud="{green_cloud}"
-        transform_pose="{target_pose}"
-        output_cloud="{output_cloud_aligned}"
-      />
-      <Action ID="SendPointCloudToUI" point_cloud="{output_cloud_aligned}" />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Look at table" _collapsed="true"/>
+      <Action ID="CreateStampedPose" position_xyz="0.02;.75;0.58" orientation_xyzw="0;0;0;1" stamped_pose="{stamped_pose}" name="Initial Guess"/>
+      <SubTree ID="Load Mesh as Red Point Cloud" _collapsed="false" output_cloud="{red_cloud}" initial_pose="{stamped_pose}" point_cloud="{red_cloud}"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{red_cloud}"/>
+      <Action ID="GetPointCloud" topic_name="/wrist_camera/points" message_out="{wrist_point_cloud}"/>
+      <Action ID="TransformPointCloudFrame" input_cloud="{wrist_point_cloud}" output_cloud="{wrist_point_cloud}"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{wrist_point_cloud}"/>
+      <Action ID="RegisterPointClouds" target_point_cloud="{wrist_point_cloud}" base_point_cloud="{red_cloud}" max_correspondence_distance="0.5" max_iterations="100"/>
+      <SubTree ID="Load Mesh as Green Point Cloud" _collapsed="false" output_cloud="{green_cloud}" initial_pose="{stamped_pose}" point_cloud="{green_cloud}"/>
+      <Action ID="TransformPointCloud" input_cloud="{green_cloud}" transform_pose="{target_pose}" output_cloud="{output_cloud_aligned}"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{output_cloud_aligned}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Register CAD Part">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/request_teleoperation.xml b/src/lab_sim/objectives/request_teleoperation.xml
index 22c306c3..1f178a6a 100644
--- a/src/lab_sim/objectives/request_teleoperation.xml
+++ b/src/lab_sim/objectives/request_teleoperation.xml
@@ -1,62 +1,32 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Request Teleoperation">
   <!--The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message.-->
-  <BehaviorTree
-    ID="Request Teleoperation"
-    _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective."
-    _favorite="false"
-    _hardcoded="false"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Request Teleoperation" _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective." _favorite="false" _hardcoded="false" _subtreeOnly="true">
     <Control ID="Sequence">
-      <Action ID="Script" code="teleop_mode := 0" />
+      <Action ID="Script" code="teleop_mode := 0"/>
       <Control ID="Parallel" success_count="1" failure_count="1">
-        <Action
-          ID="DoTeleoperateAction"
-          enable_user_interaction="{enable_user_interaction}"
-          user_interaction_prompt="{user_interaction_prompt}"
-          initial_teleop_mode="{initial_teleop_mode}"
-          current_teleop_mode="{teleop_mode}"
-        />
+        <Action ID="DoTeleoperateAction" enable_user_interaction="{enable_user_interaction}" user_interaction_prompt="{user_interaction_prompt}" initial_teleop_mode="{initial_teleop_mode}" current_teleop_mode="{teleop_mode}"/>
         <Decorator ID="KeepRunningUntilFailure">
           <Control ID="Sequence">
             <!--Closing and opening the gripper-->
             <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 7">
-              <SubTree ID="Close Gripper" />
+              <SubTree ID="Close Gripper"/>
             </Decorator>
             <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 6">
-              <SubTree ID="Open Gripper" />
+              <SubTree ID="Open Gripper"/>
             </Decorator>
             <!--Joint sliders interpolate to joint state-->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 5">
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrieveJointStateParameter"
-                      timeout_sec="-1"
-                      joint_state="{target_joint_state}"
-                    />
-                    <SubTree
-                      ID="Interpolate to Joint State"
-                      _collapsed="false"
-                      target_joint_state="{target_joint_state}"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
+                    <SubTree ID="Interpolate to Joint State" _collapsed="false" target_joint_state="{target_joint_state}"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
@@ -66,31 +36,13 @@
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrievePoseParameter"
-                      timeout_sec="-1"
-                      pose="{target_pose}"
-                    />
-                    <SubTree
-                      ID="Move to Pose"
-                      _collapsed="false"
-                      target_pose="{target_pose}"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrievePoseParameter" timeout_sec="-1" pose="{target_pose}"/>
+                    <SubTree ID="Move to Pose" _collapsed="false" target_pose="{target_pose}"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
@@ -100,56 +52,23 @@
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrieveJointStateParameter"
-                      timeout_sec="-1"
-                      joint_state="{target_joint_state}"
-                    />
-                    <SubTree
-                      ID="Move to Joint State"
-                      _collapsed="false"
-                      target_joint_state="{target_joint_state}"
-                      link_padding="0.0"
-                      acceleration_scale_factor="1.0"
-                      controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-                      controller_names="joint_trajectory_controller"
-                      joint_group_name="manipulator"
-                      keep_orientation="false"
-                      keep_orientation_link_names="grasp_link"
-                      keep_orientation_tolerance="0.05"
-                      velocity_scale_factor="1.0"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
+                    <SubTree ID="Move to Joint State" _collapsed="false" target_joint_state="{target_joint_state}" link_padding="0.0" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.05" velocity_scale_factor="1.0"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
             </Decorator>
             <!--Cartesian and joint jogging-->
             <Control ID="Sequence" _while="teleop_mode == 2">
-              <Action
-                ID="TeleoperateTwist"
-                controller_name="servo_controller"
-              />
+              <Action ID="TeleoperateTwist" controller_name="servo_controller"/>
             </Control>
             <Control ID="Sequence" _while="teleop_mode == 1">
-              <Action
-                ID="TeleoperateJointJog"
-                controller_name="servo_controller"
-              />
+              <Action ID="TeleoperateJointJog" controller_name="servo_controller"/>
             </Control>
           </Control>
         </Decorator>
@@ -158,11 +77,11 @@
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Request Teleoperation">
-      <input_port name="enable_user_interaction" default="false" />
-      <input_port name="initial_teleop_mode" default="3" />
-      <input_port name="user_interaction_prompt" default="Teleoperate" />
+      <input_port name="enable_user_interaction" default="false"/>
+      <input_port name="initial_teleop_mode" default="3"/>
+      <input_port name="user_interaction_prompt" default="Teleoperate"/>
       <MetadataFields>
-        <Metadata subcategory="User Input" />
+        <Metadata subcategory="User Input"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/sample_april_tag.xml b/src/lab_sim/objectives/sample_april_tag.xml
index f9a0e557..81500872 100644
--- a/src/lab_sim/objectives/sample_april_tag.xml
+++ b/src/lab_sim/objectives/sample_april_tag.xml
@@ -1,69 +1,31 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Sample April Tag">
   <!--//////////-->
-  <BehaviorTree
-    ID="Sample April Tag"
-    _description="Samples the pose of an AprilTag and returns the average measurement."
-    _favorite="false"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Sample April Tag" _description="Samples the pose of an AprilTag and returns the average measurement." _favorite="false" _subtreeOnly="true">
     <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_camera/camera_info"
-          message_out="{camera_info}"
-        />
+        <Action ID="LoadObjectiveParameters" config_file_name="{apriltag_config}" parameters="{parameters}"/>
+        <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" message_out="{camera_info}"/>
         <Decorator ID="RetryUntilSuccessful" num_attempts="5">
           <Control ID="Sequence">
-            <Action
-              ID="GetImage"
-              topic_name="/wrist_camera/color"
-              message_out="{image}"
-            />
-            <Action
-              ID="DetectAprilTags"
-              image="{image}"
-              camera_info="{camera_info}"
-              parameters="{parameters}"
-              detections="{detections}"
-            />
-            <Action
-              ID="GetDetectionPose"
-              detections="{detections}"
-              target_id="{tag_id}"
-              target_label=""
-              detection_pose="{detection_pose}"
-            />
+            <Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}"/>
+            <Action ID="DetectAprilTags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/>
+            <Action ID="GetDetectionPose" detections="{detections}" target_id="{tag_id}" target_label="" detection_pose="{detection_pose}"/>
           </Control>
         </Decorator>
-        <Action
-          ID="AveragePoseStamped"
-          pose_sample="{detection_pose}"
-          avg_pose="{avg_pose}"
-          num_samples="{num_samples}"
-          run_continuously="false"
-        />
+        <Action ID="AveragePoseStamped" pose_sample="{detection_pose}" avg_pose="{avg_pose}" num_samples="{num_samples}" run_continuously="false"/>
       </Control>
     </Decorator>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Sample April Tag">
       <MetadataFields>
-        <Metadata subcategory="Perception - 2D Image" />
+        <Metadata subcategory="Perception - 2D Image"/>
       </MetadataFields>
-      <output_port name="avg_pose" default="{avg_pose}" />
-      <input_port name="tag_id" default="-1" />
-      <input_port
-        name="apriltag_config"
-        default="51mm_apriltag_detection_config.yaml"
-      />
-      <input_port name="num_samples" default="5" />
+      <output_port name="avg_pose" default="{avg_pose}"/>
+      <input_port name="tag_id" default="-1"/>
+      <input_port name="apriltag_config" default="51mm_apriltag_detection_config.yaml"/>
+      <input_port name="num_samples" default="5"/>
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/segment_image_from_text_prompt.xml b/src/lab_sim/objectives/segment_image_from_text_prompt.xml
index 07c232a7..51184e21 100644
--- a/src/lab_sim/objectives/segment_image_from_text_prompt.xml
+++ b/src/lab_sim/objectives/segment_image_from_text_prompt.xml
@@ -1,37 +1,17 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Segment Image from Text Prompt">
   <!--//////////-->
-  <BehaviorTree
-    ID="Segment Image from Text Prompt"
-    _subtreeOnly="false"
-    _description="Run text based segmentation and visualize he masks."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Segment Image from Text Prompt" _subtreeOnly="false" _description="Run text based segmentation and visualize he masks." _favorite="true">
     <Control ID="Sequence">
-      <Action
-        ID="SwitchUIPrimaryView"
-        primary_view_name="/masks_visualization"
-      />
-      <SubTree
-        ID="Segment Image from No Negative Text Prompt Subtree"
-        _collapsed="true"
-        clip_model_path="models/clip.onnx"
-        clipseg_model_path="models/clipseg.onnx"
-        erosion_size="2"
-        masks_visualization_topic="/masks_visualization"
-        model_package="lab_sim"
-        threshold="0.28"
-        prompts="an object"
-        image_topic_name="/wrist_camera/color"
-        masks2d="{masks2d}"
-      />
+      <Action ID="SwitchUIPrimaryView" primary_view_name="/masks_visualization"/>
+      <SubTree ID="Segment Image from No Negative Text Prompt Subtree" _collapsed="true" clip_model_path="models/clip.onnx" clipseg_model_path="models/clipseg.onnx" erosion_size="2" masks_visualization_topic="/masks_visualization" model_package="lab_sim" threshold="0.28" prompts="an object" image_topic_name="/wrist_camera/color" masks2d="{masks2d}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Segment Image from Text Prompt">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/segment_point_cloud_from_clicked_point.xml b/src/lab_sim/objectives/segment_point_cloud_from_clicked_point.xml
index 2f74d9c5..1dc97e0c 100644
--- a/src/lab_sim/objectives/segment_point_cloud_from_clicked_point.xml
+++ b/src/lab_sim/objectives/segment_point_cloud_from_clicked_point.xml
@@ -1,33 +1,16 @@
-<?xml version="1.0" encoding="utf-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Segment Point Cloud from Clicked Point"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Segment Point Cloud from Clicked Point">
   <!--//////////-->
-  <BehaviorTree
-    ID="Segment Point Cloud from Clicked Point"
-    _description="Captures a point cloud and requests the user to click on three objects in the image to be segmented. The point cloud is then filtered to only include the selected objects."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Segment Point Cloud from Clicked Point" _description="Captures a point cloud and requests the user to click on three objects in the image to be segmented. The point cloud is then filtered to only include the selected objects." _favorite="true">
     <Control ID="Sequence">
-      <SubTree
-        ID="Segment Point Cloud from Clicked Point Subtree"
-        _collapsed="true"
-        camera_topic_name="/wrist_camera/camera_info"
-        decoder_model_path="models/decoder.onnx"
-        encoder_model_path="models/sam2_hiera_large_encoder.onnx"
-        image_topic_name="/wrist_camera/color"
-        masks_visualization_topic="/masks_visualization"
-        model_package="lab_sim"
-        points_topic_name="/wrist_camera/points"
-      />
+      <SubTree ID="Segment Point Cloud from Clicked Point Subtree" _collapsed="true" camera_topic_name="/wrist_camera/camera_info" decoder_model_path="models/decoder.onnx" encoder_model_path="models/sam2_hiera_large_encoder.onnx" image_topic_name="/wrist_camera/color" masks_visualization_topic="/masks_visualization" model_package="lab_sim" points_topic_name="/wrist_camera/points"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Segment Point Cloud from Clicked Point">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/segment_point_cloud_from_text_prompt.xml b/src/lab_sim/objectives/segment_point_cloud_from_text_prompt.xml
index 2eaee266..865ae376 100644
--- a/src/lab_sim/objectives/segment_point_cloud_from_text_prompt.xml
+++ b/src/lab_sim/objectives/segment_point_cloud_from_text_prompt.xml
@@ -1,40 +1,16 @@
-<?xml version="1.0" encoding="utf-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Segment Point Cloud from Text Prompt"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Segment Point Cloud from Text Prompt">
   <!--//////////-->
-  <BehaviorTree
-    ID="Segment Point Cloud from Text Prompt"
-    _description="Captures a point cloud and requests the user to click an object in the image to be segmented. The point cloud is then filtered to only include the selected object."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Segment Point Cloud from Text Prompt" _description="Captures a point cloud and requests the user to click an object in the image to be segmented. The point cloud is then filtered to only include the selected object." _favorite="true">
     <Control ID="Sequence">
-      <SubTree
-        ID="Segment Point Cloud from Text Prompt Subtree"
-        _collapsed="true"
-        camera_topic_name="/wrist_camera/camera_info"
-        decoder_model_path="models/decoder.onnx"
-        encoder_model_path="models/sam2_hiera_large_encoder.onnx"
-        image_topic_name="/wrist_camera/color"
-        masks_visualization_topic="/masks_visualization"
-        model_package="lab_sim"
-        points_topic_name="/wrist_camera/points"
-        clip_model_path="models/clip.onnx"
-        clipseg_model_path="models/clipseg.onnx"
-        erosion_size="2"
-        prompts="an object"
-        threshold="0.28"
-        masks3d="{masks3d}"
-        point_cloud="{point_cloud}"
-      />
+      <SubTree ID="Segment Point Cloud from Text Prompt Subtree" _collapsed="true" camera_topic_name="/wrist_camera/camera_info" decoder_model_path="models/decoder.onnx" encoder_model_path="models/sam2_hiera_large_encoder.onnx" image_topic_name="/wrist_camera/color" masks_visualization_topic="/masks_visualization" model_package="lab_sim" points_topic_name="/wrist_camera/points" clip_model_path="models/clip.onnx" clipseg_model_path="models/clipseg.onnx" erosion_size="2" prompts="an object" threshold="0.28" masks3d="{masks3d}" point_cloud="{point_cloud}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Segment Point Cloud from Text Prompt">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/take_picture_with_wrist_camera.xml b/src/lab_sim/objectives/take_picture_with_wrist_camera.xml
index c5bd4110..abf1411d 100644
--- a/src/lab_sim/objectives/take_picture_with_wrist_camera.xml
+++ b/src/lab_sim/objectives/take_picture_with_wrist_camera.xml
@@ -1,42 +1,21 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Take Wrist Camera Image">
   <!--//////////-->
-  <BehaviorTree
-    ID="Take Wrist Camera Image"
-    _subtreeOnly="true"
-    _description="Loads wrist camera parameters and records color image."
-  >
+  <BehaviorTree ID="Take Wrist Camera Image" _subtreeOnly="true" _description="Loads wrist camera parameters and records color image.">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="apriltag_detection_config.yaml"
-        parameters="{wrist_camera_parameters}"
-      />
-      <Action
-        ID="GetCameraInfo"
-        topic_name="/wrist_camera/camera_info"
-        message_out="{wrist_camera_info}"
-        timeout_sec="5.000000"
-      />
-      <Action
-        ID="GetImage"
-        topic_name="/wrist_camera/color"
-        message_out="{wrist_camera_image}"
-        timeout_sec="5.000000"
-      />
+      <Action ID="LoadObjectiveParameters" config_file_name="apriltag_detection_config.yaml" parameters="{wrist_camera_parameters}"/>
+      <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" message_out="{wrist_camera_info}" timeout_sec="5.000000"/>
+      <Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{wrist_camera_image}" timeout_sec="5.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Take Wrist Camera Image">
-      <inout_port name="wrist_camera_image" default="{wrist_camera_image}" />
-      <inout_port name="wrist_camera_info" default="{wrist_camera_info}" />
-      <inout_port
-        name="wrist_camera_parameters"
-        default="{wrist_camera_parameters}"
-      />
+      <inout_port name="wrist_camera_image" default="{wrist_camera_image}"/>
+      <inout_port name="wrist_camera_info" default="{wrist_camera_info}"/>
+      <inout_port name="wrist_camera_parameters" default="{wrist_camera_parameters}"/>
       <MetadataFields>
-        <Metadata subcategory="Perception - 2D Image" />
-        <Metadata runnable="false" />
+        <Metadata subcategory="Perception - 2D Image"/>
+        <Metadata runnable="false"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/take_snap.xml b/src/lab_sim/objectives/take_snap.xml
index 85a0470e..dab932da 100644
--- a/src/lab_sim/objectives/take_snap.xml
+++ b/src/lab_sim/objectives/take_snap.xml
@@ -1,22 +1,18 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Take scene camera snapshot">
   <!--//////////-->
-  <BehaviorTree
-    ID="Take scene camera snapshot"
-    _description="Take a pointcloud snapshot of the scene with a depth camera"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Take scene camera snapshot" _description="Take a pointcloud snapshot of the scene with a depth camera" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Clear out old snapshot data -->
-      <Action ID="ClearSnapshot" />
-      <Action ID="GetPointCloud" topic_name="/scene_camera/points" />
-      <Action ID="SendPointCloudToUI" />
+      <Action ID="ClearSnapshot"/>
+      <Action ID="GetPointCloud" topic_name="/scene_camera/points"/>
+      <Action ID="SendPointCloudToUI"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Take scene camera snapshot">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/wrist_snap.xml b/src/lab_sim/objectives/wrist_snap.xml
index 81d3c072..39091093 100644
--- a/src/lab_sim/objectives/wrist_snap.xml
+++ b/src/lab_sim/objectives/wrist_snap.xml
@@ -1,21 +1,17 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Take wrist camera snapshot">
   <!--//////////-->
-  <BehaviorTree
-    ID="Take wrist camera snapshot"
-    _description="Capture and visualise a pointcloud with the wrist camera"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Take wrist camera snapshot" _description="Capture and visualise a pointcloud with the wrist camera" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Do not clear snapshots first, so we can take multiple snapshots from different angles -->
-      <Action ID="GetPointCloud" topic_name="/wrist_camera/points" />
-      <Action ID="SendPointCloudToUI" />
+      <Action ID="GetPointCloud" topic_name="/wrist_camera/points"/>
+      <Action ID="SendPointCloudToUI"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Take wrist camera snapshot">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/package.xml b/src/lab_sim/package.xml
index d78b3f6d..376061bd 100644
--- a/src/lab_sim/package.xml
+++ b/src/lab_sim/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>lab_sim</name>
   <version>6.5.0</version>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/launch/agent_bridge.launch.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/launch/agent_bridge.launch.xml
index b2aa7543..8eeec7d9 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/launch/agent_bridge.launch.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/launch/agent_bridge.launch.xml
@@ -1,6 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
 </launch>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/clear_snapshot.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/clear_snapshot.xml
index 5ac3f027..ee491e78 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/clear_snapshot.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/clear_snapshot.xml
@@ -1,16 +1,13 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Clear Snapshot">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Clear Snapshot"
-    _description="Clear the point cloud snapshot"
-  >
-    <Action ID="ClearSnapshot" />
+  <BehaviorTree ID="Clear Snapshot" _description="Clear the point cloud snapshot">
+    <Action ID="ClearSnapshot"/>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Clear Snapshot">
       <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
+        <Metadata subcategory="Perception - 3D Point Cloud"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/close_gripper.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/close_gripper.xml
index 27703f75..7e9ee13c 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/close_gripper.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/close_gripper.xml
@@ -1,18 +1,14 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Close Gripper">
   <BehaviorTree ID="Close Gripper" _description="Close the gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.7929"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Close Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml
index 70fa45ba..d7dde188 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml
@@ -1,33 +1,21 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Get IMarker Pose From Mesh Visualization"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Get IMarker Pose From Mesh Visualization">
   <!--//////////-->
-  <BehaviorTree
-    ID="Get IMarker Pose From Mesh Visualization"
-    _description="Visualize a mesh in the UI and use an IMarker to get a grasp pose based on that mesh"
-    _favorite="false"
-  >
+  <BehaviorTree ID="Get IMarker Pose From Mesh Visualization" _description="Visualize a mesh in the UI and use an IMarker to get a grasp pose based on that mesh" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="VisualizeMesh" marker_lifetime="0.0" />
-      <Action ID="AddPoseStampedToVector" input="{mesh_pose}" />
-      <Action
-        ID="AdjustPoseWithIMarker"
-        initial_poses="{pose_stamped_vector}"
-        adjusted_poses="{adjusted_poses}"
-        prompts="Adjust IMarker for Poses relative to the mesh"
-      />
+      <Action ID="VisualizeMesh" marker_lifetime="0.0"/>
+      <Action ID="AddPoseStampedToVector" input="{mesh_pose}"/>
+      <Action ID="AdjustPoseWithIMarker" initial_poses="{pose_stamped_vector}" adjusted_poses="{adjusted_poses}" prompts="Adjust IMarker for Poses relative to the mesh"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Get IMarker Pose From Mesh Visualization">
-      <output_port name="adjusted_poses" default="{adjusted_poses}" />
-      <input_port name="mesh_path" default="{mesh_path}" />
-      <input_port name="mesh_pose" default="{mesh_pose}" />
-      <input_port name="_collapsed" default="false" />
+      <output_port name="adjusted_poses" default="{adjusted_poses}"/>
+      <input_port name="mesh_path" default="{mesh_path}"/>
+      <input_port name="mesh_pose" default="{mesh_pose}"/>
+      <input_port name="_collapsed" default="false"/>
       <MetadataFields>
-        <Metadata subcategory="User Input" />
+        <Metadata subcategory="User Input"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_along_path.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_along_path.xml
index c1951a33..ebc27f67 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_along_path.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_along_path.xml
@@ -1,103 +1,25 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move Along Path">
   <!--//////////-->
-  <BehaviorTree
-    ID="Move Along Path"
-    _description="Moves the robot tip on a rectangular path."
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Move Along Path" _description="Moves the robot tip on a rectangular path." _favorite="true" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <Control ID="Sequence">
-        <Action
-          ID="ActivateControllers"
-          controller_names="/joint_trajectory_controller"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.3;0.1;0.5"
-          orientation_xyzw="0.707;0.707;0.0;.0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.5;0.1;0.5"
-          orientation_xyzw="0.707;0.707;0.0;.0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.5;-0.1;0.5"
-          orientation_xyzw="0.707;0.707;0.0;.0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.3;-0.1;0.5"
-          orientation_xyzw="0.707;0.707;0.0;.0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
+        <Action ID="ActivateControllers" controller_names="/joint_trajectory_controller"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.3;0.1;0.5" orientation_xyzw="0.707;0.707;0.0;.0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.5;0.1;0.5" orientation_xyzw="0.707;0.707;0.0;.0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.5;-0.1;0.5" orientation_xyzw="0.707;0.707;0.0;.0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.3;-0.1;0.5" orientation_xyzw="0.707;0.707;0.0;.0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
       </Control>
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
-          <Action
-            ID="PlanCartesianPath"
-            path="{pose_stamped_vector}"
-            planning_group_name="manipulator"
-            tip_link="end_effector_link"
-            tip_offset="0.0;0.0;0.0;0.0;0.0;0.0"
-            position_only="false"
-            blending_radius="0.02"
-            velocity_scale_factor="0.5"
-            acceleration_scale_factor="0.5"
-            trajectory_sampling_rate="100"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-            debug_solution="{debug_solution}"
-          />
-          <Action
-            ID="GetCurrentPlanningScene"
-            planning_scene_msg="{planning_scene_msg}"
-          />
-          <Action
-            ID="ValidateTrajectory"
-            planning_scene_msg="{planning_scene_msg}"
-            planning_group_name="manipulator"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-            joint_space_step="0.05"
-            cartesian_space_step="0.02"
-            debug_solution="{debug_solution}"
-          />
-          <Action
-            ID="ExecuteFollowJointTrajectory"
-            execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-            goal_position_tolerance="0.000000"
-            goal_time_tolerance="0.000000"
-          />
+          <Action ID="PlanCartesianPath" path="{pose_stamped_vector}" planning_group_name="manipulator" tip_link="end_effector_link" tip_offset="0.0;0.0;0.0;0.0;0.0;0.0" position_only="false" blending_radius="0.02" velocity_scale_factor="0.5" acceleration_scale_factor="0.5" trajectory_sampling_rate="100" joint_trajectory_msg="{joint_trajectory_msg}" debug_solution="{debug_solution}"/>
+          <Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene_msg}"/>
+          <Action ID="ValidateTrajectory" planning_scene_msg="{planning_scene_msg}" planning_group_name="manipulator" joint_trajectory_msg="{joint_trajectory_msg}" joint_space_step="0.05" cartesian_space_step="0.02" debug_solution="{debug_solution}"/>
+          <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" joint_trajectory_msg="{joint_trajectory_msg}" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000"/>
         </Control>
       </Decorator>
     </Control>
@@ -105,7 +27,7 @@
   <TreeNodesModel>
     <SubTree ID="Move Along Path">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_joint_state.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_joint_state.xml
index 08e507c2..88869285 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_joint_state.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_joint_state.xml
@@ -1,47 +1,25 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
-  <BehaviorTree
-    ID="Move to Joint State"
-    _description="Move to a specified joint state"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Move to Joint State" _description="Move to a specified joint state" _subtreeOnly="true">
     <Control ID="Sequence">
-      <Action ID="ActivateControllers" controller_names="{controller_names}" />
-      <Action
-        ID="PlanToJointGoal"
-        joint_goal="{target_joint_state}"
-        keep_orientation_tolerance="{keep_orientation_tolerance}"
-        link_padding="{link_padding}"
-        keep_orientation="{keep_orientation}"
-        acceleration_scale_factor="{acceleration_scale_factor}"
-        velocity_scale_factor="{velocity_scale_factor}"
-        planning_group_name="{joint_group_name}"
-      />
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="{controller_action_server}"
-      />
+      <Action ID="ActivateControllers" controller_names="{controller_names}"/>
+      <Action ID="PlanToJointGoal" joint_goal="{target_joint_state}" keep_orientation_tolerance="{keep_orientation_tolerance}" link_padding="{link_padding}" keep_orientation="{keep_orientation}" acceleration_scale_factor="{acceleration_scale_factor}" velocity_scale_factor="{velocity_scale_factor}" planning_group_name="{joint_group_name}"/>
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="{controller_action_server}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Joint State">
-      <input_port name="target_joint_state" default="{target_joint_state}" />
-      <input_port name="joint_group_name" default="manipulator" />
-      <input_port name="link_padding" default="0.01" />
-      <input_port name="keep_orientation_tolerance" default="0.05" />
-      <input_port name="keep_orientation" default="false" />
-      <input_port name="velocity_scale_factor" default="1.0" />
-      <input_port name="acceleration_scale_factor" default="1.0" />
-      <input_port
-        name="controller_names"
-        default="joint_trajectory_controller"
-      />
-      <input_port
-        name="controller_action_server"
-        default="/joint_trajectory_controller/follow_joint_trajectory"
-      />
+      <input_port name="target_joint_state" default="{target_joint_state}"/>
+      <input_port name="joint_group_name" default="manipulator"/>
+      <input_port name="link_padding" default="0.01"/>
+      <input_port name="keep_orientation_tolerance" default="0.05"/>
+      <input_port name="keep_orientation" default="false"/>
+      <input_port name="velocity_scale_factor" default="1.0"/>
+      <input_port name="acceleration_scale_factor" default="1.0"/>
+      <input_port name="controller_names" default="joint_trajectory_controller"/>
+      <input_port name="controller_action_server" default="/joint_trajectory_controller/follow_joint_trajectory"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/open_gripper.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/open_gripper.xml
index 280fd7e6..be44be79 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/open_gripper.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/open_gripper.xml
@@ -1,19 +1,15 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Open Gripper">
   <!-- ////////// -->
   <BehaviorTree ID="Open Gripper" _description="Open the gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.0"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Open Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/reactivate_gripper.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/reactivate_gripper.xml
index 88383610..493a70c1 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/reactivate_gripper.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/reactivate_gripper.xml
@@ -1,18 +1,15 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Reactivate Gripper">
   <!-- ////////// -->
   <BehaviorTree ID="Reactivate Gripper" _description="Reactivate the gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="CallTriggerService"
-        service_name="/robotiq_activation_controller/reactivate_gripper"
-      />
+      <Action ID="CallTriggerService" service_name="/robotiq_activation_controller/reactivate_gripper"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Reactivate Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/package.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/package.xml
index 249df368..30071015 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/package.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>kinova_gen3_base_config</name>
   <version>6.5.0</version>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/gen3_7dof.xml b/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/gen3_7dof.xml
index c306c66a..f6300fbc 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/gen3_7dof.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/gen3_7dof.xml
@@ -1,624 +1,217 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="gen3">
-  <compiler angle="radian" meshdir="assets" />
-  <option integrator="implicitfast" />
+  <compiler angle="radian" meshdir="assets"/>
+  <option integrator="implicitfast"/>
 
   <default>
     <!-- kinova defaults -->
     <default class="kinova">
-      <material specular="0.5" shininess="0.25" />
-      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973" />
-      <general
-        dyntype="none"
-        biastype="affine"
-        ctrlrange="-6.28 6.28"
-        forcerange="-87 87"
-      />
+      <material specular="0.5" shininess="0.25"/>
+      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
+      <general dyntype="none" biastype="affine" ctrlrange="-6.28 6.28" forcerange="-87 87"/>
       <default class="infiniteJoints">
-        <joint limited="false" />
+        <joint limited="false"/>
       </default>
       <default class="limitedJoints">
-        <joint limited="true" />
+        <joint limited="true"/>
       </default>
-      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4" />
+      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
     </default>
     <!-- robotiq 2f85 defaults -->
     <default class="2f85">
-      <mesh scale="0.001 0.001 0.001" />
-      <general biastype="affine" />
+      <mesh scale="0.001 0.001 0.001"/>
+      <general biastype="affine"/>
 
-      <joint axis="1 0 0" />
+      <joint axis="1 0 0"/>
       <default class="knuckle_link">
-        <joint
-          range="0 0.8"
-          armature="0.005"
-          damping="0.1"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="0 0.8" armature="0.005" damping="0.1" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
       <default class="finger_tip_link">
-        <joint
-          range="-0.872664 0.872664"
-          armature="0.001"
-          pos="0 -0.018 0.0065"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="-0.872664 0.872664" armature="0.001" pos="0 -0.018 0.0065" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
       <default class="inner_knuckle_link">
-        <joint
-          range="-0.29670597283 0.8"
-          armature="0.001"
-          stiffness="0.05"
-          springref="2.62"
-          damping="0.00125"
-        />
+        <joint range="-0.29670597283 0.8" armature="0.001" stiffness="0.05" springref="2.62" damping="0.00125"/>
       </default>
       <default class="finger_link">
-        <joint
-          range="-1.57 0"
-          armature="0.001"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="-1.57 0" armature="0.001" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
 
       <default class="visual">
-        <geom type="mesh" contype="0" conaffinity="0" group="2" />
+        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
       </default>
       <default class="collision">
-        <geom type="mesh" group="3" condim="6" />
+        <geom type="mesh" group="3" condim="6"/>
         <default class="pad_box1">
-          <geom
-            mass="0"
-            type="box"
-            pos="0 -0.0026 0.028125"
-            size="0.011 0.004 0.009375"
-            friction="0.7"
-            solimp="0.95 0.99 0.001"
-            solref="0.004 1"
-            priority="1"
-            rgba="0.55 0.55 0.55 1"
-          />
+          <geom mass="0" type="box" pos="0 -0.0026 0.028125" size="0.011 0.004 0.009375" friction="0.7" solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.55 0.55 0.55 1"/>
         </default>
         <default class="pad_box2">
-          <geom
-            mass="0"
-            type="box"
-            pos="0 -0.0026 0.009375"
-            size="0.011 0.004 0.009375"
-            friction="0.6"
-            solimp="0.95 0.99 0.001"
-            solref="0.004 1"
-            priority="1"
-            rgba="0.45 0.45 0.45 1"
-          />
+          <geom mass="0" type="box" pos="0 -0.0026 0.009375" size="0.011 0.004 0.009375" friction="0.6" solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.45 0.45 0.45 1"/>
         </default>
       </default>
     </default>
   </default>
 
   <asset>
-    <material name="metal" rgba="0.58 0.58 0.58 1" />
-    <material name="silicone" rgba="0.1882 0.1882 0.1882 1" />
-    <material name="white" rgba="1.0 1.0 1.0 1" />
-    <material name="gray" rgba="0.4627 0.4627 0.4627 1" />
-    <material name="black" rgba="0.149 0.149 0.149 1" />
+    <material name="metal" rgba="0.58 0.58 0.58 1"/>
+    <material name="silicone" rgba="0.1882 0.1882 0.1882 1"/>
+    <material name="white" rgba="1.0 1.0 1.0 1"/>
+    <material name="gray" rgba="0.4627 0.4627 0.4627 1"/>
+    <material name="black" rgba="0.149 0.149 0.149 1"/>
 
     <!-- Kinova collision meshes -->
-    <mesh name="base_link_c" file="kinova/base_link.STL" />
-    <mesh name="shoulder_link_c" file="kinova/shoulder_link.STL" />
-    <mesh name="half_arm_1_link_c" file="kinova/half_arm_1_link.STL" />
-    <mesh name="half_arm_2_link_c" file="kinova/half_arm_2_link.STL" />
-    <mesh name="forearm_link_c" file="kinova/forearm_link.STL" />
-    <mesh
-      name="spherical_wrist_1_link_c"
-      file="kinova/spherical_wrist_1_link.STL"
-    />
-    <mesh
-      name="spherical_wrist_2_link_c"
-      file="kinova/spherical_wrist_2_link.STL"
-    />
-    <mesh
-      name="bracelet_with_vision_link_c"
-      file="kinova/bracelet_with_vision_link.STL"
-    />
+    <mesh name="base_link_c" file="kinova/base_link.STL"/>
+    <mesh name="shoulder_link_c" file="kinova/shoulder_link.STL"/>
+    <mesh name="half_arm_1_link_c" file="kinova/half_arm_1_link.STL"/>
+    <mesh name="half_arm_2_link_c" file="kinova/half_arm_2_link.STL"/>
+    <mesh name="forearm_link_c" file="kinova/forearm_link.STL"/>
+    <mesh name="spherical_wrist_1_link_c" file="kinova/spherical_wrist_1_link.STL"/>
+    <mesh name="spherical_wrist_2_link_c" file="kinova/spherical_wrist_2_link.STL"/>
+    <mesh name="bracelet_with_vision_link_c" file="kinova/bracelet_with_vision_link.STL"/>
     <!-- Kinova visual meshes -->
-    <mesh name="base_link" file="kinova/base_link.obj" />
-    <mesh name="shoulder_link" file="kinova/shoulder_link.obj" />
-    <mesh name="half_arm_1_link" file="kinova/half_arm_1_link.obj" />
-    <mesh name="half_arm_2_link" file="kinova/half_arm_2_link.obj" />
-    <mesh name="forearm_link" file="kinova/forearm_link.obj" />
-    <mesh
-      name="spherical_wrist_1_link"
-      file="kinova/spherical_wrist_1_link.obj"
-    />
-    <mesh
-      name="spherical_wrist_2_link"
-      file="kinova/spherical_wrist_2_link.obj"
-    />
-    <mesh
-      name="bracelet_with_vision_link"
-      file="kinova/bracelet_with_vision_link.obj"
-    />
+    <mesh name="base_link" file="kinova/base_link.obj"/>
+    <mesh name="shoulder_link" file="kinova/shoulder_link.obj"/>
+    <mesh name="half_arm_1_link" file="kinova/half_arm_1_link.obj"/>
+    <mesh name="half_arm_2_link" file="kinova/half_arm_2_link.obj"/>
+    <mesh name="forearm_link" file="kinova/forearm_link.obj"/>
+    <mesh name="spherical_wrist_1_link" file="kinova/spherical_wrist_1_link.obj"/>
+    <mesh name="spherical_wrist_2_link" file="kinova/spherical_wrist_2_link.obj"/>
+    <mesh name="bracelet_with_vision_link" file="kinova/bracelet_with_vision_link.obj"/>
     <!-- Robotiq 2f85 meshes -->
-    <mesh name="robotiq_85_base" class="2f85" file="robotiq_2f85/base.stl" />
-    <mesh
-      name="robotiq_85_knuckle_link"
-      class="2f85"
-      file="robotiq_2f85/knuckle_link.stl"
-    />
-    <mesh
-      name="robotiq_85_inner_knuckle_link"
-      class="2f85"
-      file="robotiq_2f85/inner_knuckle_link.stl"
-    />
-    <mesh
-      name="robotiq_85_finger_link"
-      class="2f85"
-      file="robotiq_2f85/finger_link.stl"
-    />
-    <mesh
-      name="robotiq_85_finger_tip_link"
-      class="2f85"
-      file="robotiq_2f85/finger_tip_link.stl"
-    />
-    <mesh name="robotiq_85_pad" class="2f85" file="robotiq_2f85/pad.stl" />
-    <mesh
-      name="robotiq_85_silicone_pad"
-      class="2f85"
-      file="robotiq_2f85/silicone_pad.stl"
-    />
-    <mesh name="rafti_finger" file="rafti_finger.obj" />
-    <mesh name="rafti_finger_collision_1" file="rafti_finger_collision_1.obj" />
-    <mesh name="rafti_finger_collision_2" file="rafti_finger_collision_2.obj" />
-    <mesh name="rafti_finger_collision_3" file="rafti_finger_collision_3.obj" />
-    <mesh name="rafti_finger_collision_4" file="rafti_finger_collision_4.obj" />
+    <mesh name="robotiq_85_base" class="2f85" file="robotiq_2f85/base.stl"/>
+    <mesh name="robotiq_85_knuckle_link" class="2f85" file="robotiq_2f85/knuckle_link.stl"/>
+    <mesh name="robotiq_85_inner_knuckle_link" class="2f85" file="robotiq_2f85/inner_knuckle_link.stl"/>
+    <mesh name="robotiq_85_finger_link" class="2f85" file="robotiq_2f85/finger_link.stl"/>
+    <mesh name="robotiq_85_finger_tip_link" class="2f85" file="robotiq_2f85/finger_tip_link.stl"/>
+    <mesh name="robotiq_85_pad" class="2f85" file="robotiq_2f85/pad.stl"/>
+    <mesh name="robotiq_85_silicone_pad" class="2f85" file="robotiq_2f85/silicone_pad.stl"/>
+    <mesh name="rafti_finger" file="rafti_finger.obj"/>
+    <mesh name="rafti_finger_collision_1" file="rafti_finger_collision_1.obj"/>
+    <mesh name="rafti_finger_collision_2" file="rafti_finger_collision_2.obj"/>
+    <mesh name="rafti_finger_collision_3" file="rafti_finger_collision_3.obj"/>
+    <mesh name="rafti_finger_collision_4" file="rafti_finger_collision_4.obj"/>
   </asset>
 
   <contact>
-    <exclude body1="base_link" body2="shoulder_link" />
+    <exclude body1="base_link" body2="shoulder_link"/>
   </contact>
 
   <worldbody>
-    <light name="top" pos="0 0 2" mode="trackcom" />
+    <light name="top" pos="0 0 2" mode="trackcom"/>
 
     <body name="base_link" gravcomp="1" pos="0 0 0.02" quat="1 0 0 0">
-      <inertial
-        pos="-0.000648 -0.000166 0.084487"
-        quat="1 0 0 0"
-        mass="1.697"
-        diaginertia="0.004622 0.004495 0.002079"
-      />
-      <geom class="visual" mesh="base_link" material="white" />
-      <geom class="collision" mesh="base_link_c" />
-      <body
-        name="shoulder_link"
-        gravcomp="1"
-        pos="0 0 0.15643"
-        quat="-3.67321e-06 1 0 0"
-      >
-        <inertial
-          pos="-2.3e-05 -0.010364 -0.07336"
-          quat="0.707051 0.0451246 -0.0453544 0.704263"
-          mass="1.3773"
-          diaginertia="0.00488868 0.00457 0.00135132"
-        />
-        <joint class="infiniteJoints" name="joint_1" armature="0.0000015" />
-        <geom class="visual" mesh="shoulder_link" material="white" />
-        <geom class="collision" mesh="shoulder_link_c" />
-        <body
-          name="half_arm_1_link"
-          gravcomp="1"
-          pos="0 0.005375 -0.12838"
-          quat="0.707105 0.707108 0 0"
-        >
-          <inertial
-            pos="-4.4e-05 -0.09958 -0.013278"
-            quat="0.482348 0.516286 -0.516862 0.483366"
-            mass="1.1636"
-            diaginertia="0.0113017 0.011088 0.00102532"
-          />
-          <joint
-            class="limitedJoints"
-            name="joint_2"
-            range="-2.41 2.41"
-            armature="0.0000015"
-          />
-          <geom class="visual" mesh="half_arm_1_link" material="white" />
-          <geom class="collision" mesh="half_arm_1_link_c" />
-          <body
-            name="half_arm_2_link"
-            gravcomp="1"
-            pos="0 -0.21038 -0.006375"
-            quat="0.707105 -0.707108 0 0"
-          >
-            <inertial
-              pos="-4.4e-05 -0.006641 -0.117892"
-              quat="0.706144 0.0213722 -0.0209128 0.707437"
-              mass="1.1636"
-              diaginertia="0.0111633 0.010932 0.00100671"
-            />
-            <joint class="infiniteJoints" name="joint_3" armature="0.0000015" />
-            <geom class="visual" mesh="half_arm_2_link" material="white" />
-            <geom class="collision" mesh="half_arm_2_link_c" />
-            <body
-              name="forearm_link"
-              gravcomp="1"
-              pos="0 0.006375 -0.21038"
-              quat="0.707105 0.707108 0 0"
-            >
-              <inertial
-                pos="-1.8e-05 -0.075478 -0.015006"
-                quat="0.483678 0.515961 -0.515859 0.483455"
-                mass="0.9302"
-                diaginertia="0.00834839 0.008147 0.000598606"
-              />
-              <joint
-                class="limitedJoints"
-                name="joint_4"
-                range="-2.66 2.66"
-                armature="0.0000015"
-              />
-              <geom class="visual" mesh="forearm_link" material="white" />
-              <geom class="collision" mesh="forearm_link_c" />
-              <body
-                name="spherical_wrist_1_link"
-                gravcomp="1"
-                pos="0 -0.20843 -0.006375"
-                quat="0.707105 -0.707108 0 0"
-              >
-                <inertial
-                  pos="1e-06 -0.009432 -0.063883"
-                  quat="0.703558 0.0707492 -0.0707492 0.703558"
-                  mass="0.6781"
-                  diaginertia="0.00165901 0.001596 0.000346988"
-                />
-                <joint
-                  class="infiniteJoints"
-                  name="joint_5"
-                  armature="0.000001928"
-                />
-                <geom
-                  class="visual"
-                  mesh="spherical_wrist_1_link"
-                  material="white"
-                />
-                <geom class="collision" mesh="spherical_wrist_1_link_c" />
-                <body
-                  name="spherical_wrist_2_link"
-                  gravcomp="1"
-                  pos="0 0.00017505 -0.10593"
-                  quat="0.707105 0.707108 0 0"
-                >
-                  <inertial
-                    pos="1e-06 -0.045483 -0.00965"
-                    quat="0.44426 0.550121 -0.550121 0.44426"
-                    mass="0.6781"
-                    diaginertia="0.00170087 0.001641 0.00035013"
-                  />
-                  <joint
-                    class="limitedJoints"
-                    name="joint_6"
-                    range="-2.23 2.23"
-                    armature="0.000001928"
-                  />
-                  <geom
-                    class="visual"
-                    mesh="spherical_wrist_2_link"
-                    material="white"
-                  />
-                  <geom class="collision" mesh="spherical_wrist_2_link_c" />
-                  <body
-                    name="bracelet_link"
-                    gravcomp="1"
-                    pos="0 -0.10593 -0.00017505"
-                    quat="0.707105 -0.707108 0 0"
-                  >
-                    <camera
-                      name="wrist_camera"
-                      pos="0.0 -0.056 -0.058"
-                      fovy="58"
-                      mode="fixed"
-                      resolution="640 480"
-                      quat="0.0 0.0 0.0 1.0"
-                    />
-                    <site
-                      name="wrist_camera_optical_frame"
-                      pos="0.0 -0.056 -0.058"
-                      quat="0.0 0.0 1.0 0.0"
-                    />
-                    <inertial
-                      pos="-9.3e-05 0.000132 -0.022905"
-                      quat="-0.0368025 0.719402 -0.0444884 0.69219"
-                      mass="0.364"
-                      diaginertia="0.00024027 0.000222769 0.000213961"
-                    />
-                    <joint
-                      class="infiniteJoints"
-                      name="joint_7"
-                      armature="0.000001928"
-                    />
-                    <geom
-                      class="visual"
-                      mesh="bracelet_with_vision_link"
-                      material="white"
-                    />
-                    <geom
-                      class="collision"
-                      mesh="bracelet_with_vision_link_c"
-                    />
+      <inertial pos="-0.000648 -0.000166 0.084487" quat="1 0 0 0" mass="1.697" diaginertia="0.004622 0.004495 0.002079"/>
+      <geom class="visual" mesh="base_link" material="white"/>
+      <geom class="collision" mesh="base_link_c"/>
+      <body name="shoulder_link" gravcomp="1" pos="0 0 0.15643" quat="-3.67321e-06 1 0 0">
+        <inertial pos="-2.3e-05 -0.010364 -0.07336" quat="0.707051 0.0451246 -0.0453544 0.704263" mass="1.3773" diaginertia="0.00488868 0.00457 0.00135132"/>
+        <joint class="infiniteJoints" name="joint_1" armature="0.0000015"/>
+        <geom class="visual" mesh="shoulder_link" material="white"/>
+        <geom class="collision" mesh="shoulder_link_c"/>
+        <body name="half_arm_1_link" gravcomp="1" pos="0 0.005375 -0.12838" quat="0.707105 0.707108 0 0">
+          <inertial pos="-4.4e-05 -0.09958 -0.013278" quat="0.482348 0.516286 -0.516862 0.483366" mass="1.1636" diaginertia="0.0113017 0.011088 0.00102532"/>
+          <joint class="limitedJoints" name="joint_2" range="-2.41 2.41" armature="0.0000015"/>
+          <geom class="visual" mesh="half_arm_1_link" material="white"/>
+          <geom class="collision" mesh="half_arm_1_link_c"/>
+          <body name="half_arm_2_link" gravcomp="1" pos="0 -0.21038 -0.006375" quat="0.707105 -0.707108 0 0">
+            <inertial pos="-4.4e-05 -0.006641 -0.117892" quat="0.706144 0.0213722 -0.0209128 0.707437" mass="1.1636" diaginertia="0.0111633 0.010932 0.00100671"/>
+            <joint class="infiniteJoints" name="joint_3" armature="0.0000015"/>
+            <geom class="visual" mesh="half_arm_2_link" material="white"/>
+            <geom class="collision" mesh="half_arm_2_link_c"/>
+            <body name="forearm_link" gravcomp="1" pos="0 0.006375 -0.21038" quat="0.707105 0.707108 0 0">
+              <inertial pos="-1.8e-05 -0.075478 -0.015006" quat="0.483678 0.515961 -0.515859 0.483455" mass="0.9302" diaginertia="0.00834839 0.008147 0.000598606"/>
+              <joint class="limitedJoints" name="joint_4" range="-2.66 2.66" armature="0.0000015"/>
+              <geom class="visual" mesh="forearm_link" material="white"/>
+              <geom class="collision" mesh="forearm_link_c"/>
+              <body name="spherical_wrist_1_link" gravcomp="1" pos="0 -0.20843 -0.006375" quat="0.707105 -0.707108 0 0">
+                <inertial pos="1e-06 -0.009432 -0.063883" quat="0.703558 0.0707492 -0.0707492 0.703558" mass="0.6781" diaginertia="0.00165901 0.001596 0.000346988"/>
+                <joint class="infiniteJoints" name="joint_5" armature="0.000001928"/>
+                <geom class="visual" mesh="spherical_wrist_1_link" material="white"/>
+                <geom class="collision" mesh="spherical_wrist_1_link_c"/>
+                <body name="spherical_wrist_2_link" gravcomp="1" pos="0 0.00017505 -0.10593" quat="0.707105 0.707108 0 0">
+                  <inertial pos="1e-06 -0.045483 -0.00965" quat="0.44426 0.550121 -0.550121 0.44426" mass="0.6781" diaginertia="0.00170087 0.001641 0.00035013"/>
+                  <joint class="limitedJoints" name="joint_6" range="-2.23 2.23" armature="0.000001928"/>
+                  <geom class="visual" mesh="spherical_wrist_2_link" material="white"/>
+                  <geom class="collision" mesh="spherical_wrist_2_link_c"/>
+                  <body name="bracelet_link" gravcomp="1" pos="0 -0.10593 -0.00017505" quat="0.707105 -0.707108 0 0">
+                    <camera name="wrist_camera" pos="0.0 -0.056 -0.058" fovy="58" mode="fixed" resolution="640 480" quat="0.0 0.0 0.0 1.0"/>
+                    <site name="wrist_camera_optical_frame" pos="0.0 -0.056 -0.058" quat="0.0 0.0 1.0 0.0"/>
+                    <inertial pos="-9.3e-05 0.000132 -0.022905" quat="-0.0368025 0.719402 -0.0444884 0.69219" mass="0.364" diaginertia="0.00024027 0.000222769 0.000213961"/>
+                    <joint class="infiniteJoints" name="joint_7" armature="0.000001928"/>
+                    <geom class="visual" mesh="bracelet_with_vision_link" material="white"/>
+                    <geom class="collision" mesh="bracelet_with_vision_link_c"/>
                     <body childclass="2f85">
                       <!-- site for end effector's f/t sensor right at the base of the robotiq gripper -->
-                      <site
-                        name="fts"
-                        size="0.01"
-                        pos="0.0 0.0 -0.0615"
-                        quat="0 0.0 1.0 0.0"
-                      />
+                      <site name="fts" size="0.01" pos="0.0 0.0 -0.0615" quat="0 0.0 1.0 0.0"/>
                       <!-- Right-hand side 4-bar linkage -->
-                      <body
-                        name="robotiq_85_base_link"
-                        gravcomp="1"
-                        pos="0 0 -0.0615"
-                        quat="0 -0.707 0.707 0"
-                      >
-                        <inertial
-                          mass="0.777441"
-                          pos="0 -2.70394e-05 0.0354675"
-                          quat="1 -0.00152849 0 0"
-                          diaginertia="0.000260285 0.000225381 0.000152708"
-                        />
-                        <geom
-                          class="visual"
-                          mesh="robotiq_85_base"
-                          material="black"
-                        />
-                        <geom class="collision" mesh="robotiq_85_base" />
-                        <site
-                          name="pinch"
-                          pos="0 0 0.145"
-                          type="sphere"
-                          group="5"
-                          rgba="0.9 0.9 0.9 1"
-                          size="0.005"
-                        />
+                      <body name="robotiq_85_base_link" gravcomp="1" pos="0 0 -0.0615" quat="0 -0.707 0.707 0">
+                        <inertial mass="0.777441" pos="0 -2.70394e-05 0.0354675" quat="1 -0.00152849 0 0" diaginertia="0.000260285 0.000225381 0.000152708"/>
+                        <geom class="visual" mesh="robotiq_85_base" material="black"/>
+                        <geom class="collision" mesh="robotiq_85_base"/>
+                        <site name="pinch" pos="0 0 0.145" type="sphere" group="5" rgba="0.9 0.9 0.9 1" size="0.005"/>
                         <!-- Right-hand side 4-bar linkage -->
-                        <body
-                          name="robotiq_85_right_knuckle_link"
-                          gravcomp="1"
-                          pos="0 0.0306011 0.054904"
-                        >
-                          <inertial
-                            mass="0.00899563"
-                            pos="2.96931e-12 0.0177547 0.00107314"
-                            quat="0.681301 0.732003 0 0"
-                            diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"
-                          />
-                          <joint
-                            name="robotiq_85_right_knuckle_joint"
-                            class="knuckle_link"
-                          />
-                          <geom
-                            class="visual"
-                            mesh="robotiq_85_knuckle_link"
-                            material="gray"
-                          />
-                          <geom
-                            class="collision"
-                            mesh="robotiq_85_knuckle_link"
-                          />
-                          <body
-                            name="robotiq_85_right_finger_link"
-                            gravcomp="1"
-                            pos="0 0.0315 -0.0041"
-                          >
-                            <inertial
-                              mass="0.0140974"
-                              pos="0 0.00301209 0.0232175"
-                              quat="0.705636 -0.0455904 0.0455904 0.705636"
-                              diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"
-                            />
-                            <joint
-                              name="robotiq_85_right_finger_joint"
-                              class="finger_link"
-                            />
-                            <geom
-                              class="visual"
-                              mesh="robotiq_85_finger_link"
-                              material="black"
-                            />
-                            <geom
-                              class="collision"
-                              mesh="robotiq_85_finger_link"
-                            />
+                        <body name="robotiq_85_right_knuckle_link" gravcomp="1" pos="0 0.0306011 0.054904">
+                          <inertial mass="0.00899563" pos="2.96931e-12 0.0177547 0.00107314" quat="0.681301 0.732003 0 0" diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
+                          <joint name="robotiq_85_right_knuckle_joint" class="knuckle_link"/>
+                          <geom class="visual" mesh="robotiq_85_knuckle_link" material="gray"/>
+                          <geom class="collision" mesh="robotiq_85_knuckle_link"/>
+                          <body name="robotiq_85_right_finger_link" gravcomp="1" pos="0 0.0315 -0.0041">
+                            <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636" diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
+                            <joint name="robotiq_85_right_finger_joint" class="finger_link"/>
+                            <geom class="visual" mesh="robotiq_85_finger_link" material="black"/>
+                            <geom class="collision" mesh="robotiq_85_finger_link"/>
                           </body>
                         </body>
-                        <body
-                          name="robotiq_85_right_inner_knuckle_link"
-                          gravcomp="1"
-                          pos="0 0.0132 0.0609"
-                        >
-                          <inertial
-                            mass="0.0221642"
-                            pos="-8.65005e-09 0.0181624 0.0212658"
-                            quat="0.663403 -0.244737 0.244737 0.663403"
-                            diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"
-                          />
-                          <joint
-                            name="robotiq_85_right_inner_knuckle_joint"
-                            class="inner_knuckle_link"
-                          />
-                          <geom
-                            class="visual"
-                            mesh="robotiq_85_inner_knuckle_link"
-                            material="black"
-                          />
-                          <geom
-                            class="collision"
-                            mesh="robotiq_85_inner_knuckle_link"
-                          />
-                          <body
-                            name="robotiq_85_right_finger_tip_link"
-                            gravcomp="1"
-                            pos="0 0.055 0.0375"
-                          >
-                            <inertial
-                              mass="0.0125222"
-                              pos="0 -0.011046 0.0124786"
-                              quat="1 0.1664 0 0"
-                              diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"
-                            />
-                            <joint
-                              name="robotiq_85_right_finger_tip_joint"
-                              class="finger_tip_link"
-                            />
-                            <geom
-                              class="visual"
-                              mesh="robotiq_85_finger_tip_link"
-                              material="black"
-                            />
-                            <geom
-                              class="collision"
-                              mesh="robotiq_85_finger_tip_link"
-                            />
+                        <body name="robotiq_85_right_inner_knuckle_link" gravcomp="1" pos="0 0.0132 0.0609">
+                          <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403" diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+                          <joint name="robotiq_85_right_inner_knuckle_joint" class="inner_knuckle_link"/>
+                          <geom class="visual" mesh="robotiq_85_inner_knuckle_link" material="black"/>
+                          <geom class="collision" mesh="robotiq_85_inner_knuckle_link"/>
+                          <body name="robotiq_85_right_finger_tip_link" gravcomp="1" pos="0 0.055 0.0375">
+                            <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0" diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
+                            <joint name="robotiq_85_right_finger_tip_joint" class="finger_tip_link"/>
+                            <geom class="visual" mesh="robotiq_85_finger_tip_link" material="black"/>
+                            <geom class="collision" mesh="robotiq_85_finger_tip_link"/>
                             <body name="right_pad" pos="0 -0.0189 0.01352">
-                              <geom class="pad_box1" name="right_pad1" />
-                              <geom class="pad_box2" name="right_pad2" />
-                              <inertial
-                                mass="0.0035"
-                                pos="0 -0.0025 0.0185"
-                                quat="0.707107 0 0 0.707107"
-                                diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"
-                              />
-                              <geom class="visual" mesh="robotiq_85_pad" />
+                              <geom class="pad_box1" name="right_pad1"/>
+                              <geom class="pad_box2" name="right_pad2"/>
+                              <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="0.707107 0 0 0.707107" diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
+                              <geom class="visual" mesh="robotiq_85_pad"/>
                               <body name="right_silicone_pad">
-                                <geom
-                                  class="visual"
-                                  mesh="robotiq_85_silicone_pad"
-                                  material="black"
-                                />
+                                <geom class="visual" mesh="robotiq_85_silicone_pad" material="black"/>
                               </body>
                             </body>
                           </body>
                         </body>
                         <!-- Left-hand side 4-bar linkage -->
-                        <body
-                          name="robotiq_85_left_knuckle_link"
-                          gravcomp="1"
-                          pos="0 -0.0306011 0.054904"
-                          quat="0 0 0 1"
-                        >
-                          <inertial
-                            mass="0.00899563"
-                            pos="0 0.0177547 0.00107314"
-                            quat="0.681301 0.732003 0 0"
-                            diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"
-                          />
-                          <joint
-                            name="robotiq_85_left_knuckle_joint"
-                            class="knuckle_link"
-                          />
-                          <geom
-                            class="visual"
-                            mesh="robotiq_85_knuckle_link"
-                            material="gray"
-                          />
-                          <geom
-                            class="collision"
-                            mesh="robotiq_85_knuckle_link"
-                          />
-                          <body
-                            name="robotiq_85_left_finger_link"
-                            gravcomp="1"
-                            pos="0 0.0315 -0.0041"
-                          >
-                            <inertial
-                              mass="0.0140974"
-                              pos="0 0.00301209 0.0232175"
-                              quat="0.705636 -0.0455904 0.0455904 0.705636"
-                              diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"
-                            />
-                            <joint
-                              name="robotiq_85_left_finger_joint"
-                              class="finger_link"
-                            />
-                            <geom
-                              class="visual"
-                              mesh="robotiq_85_finger_link"
-                              material="black"
-                            />
-                            <geom
-                              class="collision"
-                              mesh="robotiq_85_finger_link"
-                            />
+                        <body name="robotiq_85_left_knuckle_link" gravcomp="1" pos="0 -0.0306011 0.054904" quat="0 0 0 1">
+                          <inertial mass="0.00899563" pos="0 0.0177547 0.00107314" quat="0.681301 0.732003 0 0" diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
+                          <joint name="robotiq_85_left_knuckle_joint" class="knuckle_link"/>
+                          <geom class="visual" mesh="robotiq_85_knuckle_link" material="gray"/>
+                          <geom class="collision" mesh="robotiq_85_knuckle_link"/>
+                          <body name="robotiq_85_left_finger_link" gravcomp="1" pos="0 0.0315 -0.0041">
+                            <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636" diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
+                            <joint name="robotiq_85_left_finger_joint" class="finger_link"/>
+                            <geom class="visual" mesh="robotiq_85_finger_link" material="black"/>
+                            <geom class="collision" mesh="robotiq_85_finger_link"/>
                           </body>
                         </body>
-                        <body
-                          name="robotiq_85_left_inner_knuckle_link"
-                          gravcomp="1"
-                          pos="0 -0.0132 0.0609"
-                          quat="0 0 0 1"
-                        >
-                          <inertial
-                            mass="0.0221642"
-                            pos="-8.65005e-09 0.0181624 0.0212658"
-                            quat="0.663403 -0.244737 0.244737 0.663403"
-                            diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"
-                          />
-                          <joint
-                            name="robotiq_85_left_inner_knuckle_joint"
-                            class="inner_knuckle_link"
-                          />
-                          <geom
-                            class="visual"
-                            mesh="robotiq_85_inner_knuckle_link"
-                            material="black"
-                          />
-                          <geom
-                            class="collision"
-                            mesh="robotiq_85_inner_knuckle_link"
-                          />
-                          <body
-                            name="robotiq_85_left_finger_tip_link"
-                            gravcomp="1"
-                            pos="0 0.055 0.0375"
-                          >
-                            <inertial
-                              mass="0.0125222"
-                              pos="0 -0.011046 0.0124786"
-                              quat="1 0.1664 0 0"
-                              diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"
-                            />
-                            <joint
-                              name="robotiq_85_left_finger_tip_joint"
-                              class="finger_tip_link"
-                            />
-                            <geom
-                              class="visual"
-                              mesh="robotiq_85_finger_tip_link"
-                              material="black"
-                            />
-                            <geom
-                              class="collision"
-                              mesh="robotiq_85_finger_tip_link"
-                            />
+                        <body name="robotiq_85_left_inner_knuckle_link" gravcomp="1" pos="0 -0.0132 0.0609" quat="0 0 0 1">
+                          <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403" diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+                          <joint name="robotiq_85_left_inner_knuckle_joint" class="inner_knuckle_link"/>
+                          <geom class="visual" mesh="robotiq_85_inner_knuckle_link" material="black"/>
+                          <geom class="collision" mesh="robotiq_85_inner_knuckle_link"/>
+                          <body name="robotiq_85_left_finger_tip_link" gravcomp="1" pos="0 0.055 0.0375">
+                            <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0" diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
+                            <joint name="robotiq_85_left_finger_tip_joint" class="finger_tip_link"/>
+                            <geom class="visual" mesh="robotiq_85_finger_tip_link" material="black"/>
+                            <geom class="collision" mesh="robotiq_85_finger_tip_link"/>
                             <body name="left_pad" pos="0 -0.0189 0.01352">
-                              <geom class="pad_box1" name="left_pad1" />
-                              <geom class="pad_box2" name="left_pad2" />
-                              <inertial
-                                mass="0.0035"
-                                pos="0 -0.0025 0.0185"
-                                quat="1 0 0 1"
-                                diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"
-                              />
-                              <geom class="visual" mesh="robotiq_85_pad" />
+                              <geom class="pad_box1" name="left_pad1"/>
+                              <geom class="pad_box2" name="left_pad2"/>
+                              <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="1 0 0 1" diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
+                              <geom class="visual" mesh="robotiq_85_pad"/>
                               <body name="left_silicone_pad">
-                                <geom
-                                  class="visual"
-                                  mesh="robotiq_85_silicone_pad"
-                                  material="black"
-                                />
+                                <geom class="visual" mesh="robotiq_85_silicone_pad" material="black"/>
                               </body>
                             </body>
                           </body>
@@ -636,41 +229,23 @@
   </worldbody>
 
   <actuator>
-    <position class="kinova" name="joint_1" joint="joint_1" kp="800" kv="260" />
-    <position class="kinova" name="joint_2" joint="joint_2" kp="800" kv="260" />
-    <position class="kinova" name="joint_3" joint="joint_3" kp="260" kv="86" />
-    <position class="kinova" name="joint_4" joint="joint_4" kp="260" kv="86" />
-    <position class="kinova" name="joint_5" joint="joint_5" kp="130" kv="45" />
-    <position class="kinova" name="joint_6" joint="joint_6" kp="130" kv="45" />
-    <position class="kinova" name="joint_7" joint="joint_7" kp="130" kv="45" />
+    <position class="kinova" name="joint_1" joint="joint_1" kp="800" kv="260"/>
+    <position class="kinova" name="joint_2" joint="joint_2" kp="800" kv="260"/>
+    <position class="kinova" name="joint_3" joint="joint_3" kp="260" kv="86"/>
+    <position class="kinova" name="joint_4" joint="joint_4" kp="260" kv="86"/>
+    <position class="kinova" name="joint_5" joint="joint_5" kp="130" kv="45"/>
+    <position class="kinova" name="joint_6" joint="joint_6" kp="130" kv="45"/>
+    <position class="kinova" name="joint_7" joint="joint_7" kp="130" kv="45"/>
   </actuator>
 
   <!-- Robotiq gripper contact -->
   <contact>
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_left_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_right_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_left_inner_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_right_inner_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_right_finger_link"
-      body2="robotiq_85_right_finger_tip_link"
-    />
-    <exclude
-      body1="robotiq_85_left_finger_link"
-      body2="robotiq_85_left_finger_tip_link"
-    />
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_left_knuckle_link"/>
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_right_knuckle_link"/>
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_left_inner_knuckle_link"/>
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_right_inner_knuckle_link"/>
+    <exclude body1="robotiq_85_right_finger_link" body2="robotiq_85_right_finger_tip_link"/>
+    <exclude body1="robotiq_85_left_finger_link" body2="robotiq_85_left_finger_tip_link"/>
   </contact>
 
   <!--
@@ -681,49 +256,23 @@
   -->
   <tendon>
     <fixed name="split">
-      <joint joint="robotiq_85_right_knuckle_joint" coef="0.5" />
-      <joint joint="robotiq_85_left_knuckle_joint" coef="0.5" />
+      <joint joint="robotiq_85_right_knuckle_joint" coef="0.5"/>
+      <joint joint="robotiq_85_left_knuckle_joint" coef="0.5"/>
     </fixed>
   </tendon>
 
   <equality>
-    <connect
-      anchor="0 0 0"
-      body1="robotiq_85_right_finger_tip_link"
-      body2="robotiq_85_right_finger_link"
-      solimp="0.95 0.99 0.001"
-      solref="0.005 1"
-    />
-    <connect
-      anchor="0 0 0"
-      body1="robotiq_85_left_finger_tip_link"
-      body2="robotiq_85_left_finger_link"
-      solimp="0.95 0.99 0.001"
-      solref="0.005 1"
-    />
-    <joint
-      joint1="robotiq_85_left_knuckle_joint"
-      joint2="robotiq_85_right_knuckle_joint"
-      polycoef="0 1 0 0 0"
-      solimp="0.95 0.99 0.001"
-      solref="0.005 1"
-    />
+    <connect anchor="0 0 0" body1="robotiq_85_right_finger_tip_link" body2="robotiq_85_right_finger_link" solimp="0.95 0.99 0.001" solref="0.005 1"/>
+    <connect anchor="0 0 0" body1="robotiq_85_left_finger_tip_link" body2="robotiq_85_left_finger_link" solimp="0.95 0.99 0.001" solref="0.005 1"/>
+    <joint joint1="robotiq_85_left_knuckle_joint" joint2="robotiq_85_right_knuckle_joint" polycoef="0 1 0 0 0" solimp="0.95 0.99 0.001" solref="0.005 1"/>
   </equality>
 
   <actuator>
-    <position
-      class="2f85"
-      name="robotiq_85_left_knuckle_joint"
-      tendon="split"
-      forcerange="-5 5"
-      ctrlrange="0 .8"
-      kp="100"
-      kv="100"
-    />
+    <position class="2f85" name="robotiq_85_left_knuckle_joint" tendon="split" forcerange="-5 5" ctrlrange="0 .8" kp="100" kv="100"/>
   </actuator>
 
   <sensor>
-    <force site="fts" />
-    <torque site="fts" />
+    <force site="fts"/>
+    <torque site="fts"/>
   </sensor>
 </mujoco>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/gen3_7dof_rafti_fingers.xml b/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/gen3_7dof_rafti_fingers.xml
index 41444a8a..c758b320 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/gen3_7dof_rafti_fingers.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/gen3_7dof_rafti_fingers.xml
@@ -1,694 +1,235 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="gen3">
-  <compiler angle="radian" meshdir="assets" />
-  <option integrator="implicitfast" />
+  <compiler angle="radian" meshdir="assets"/>
+  <option integrator="implicitfast"/>
 
   <default>
     <!-- kinova defaults -->
     <default class="kinova">
-      <material specular="0.5" shininess="0.25" />
-      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973" />
-      <general
-        dyntype="none"
-        biastype="affine"
-        ctrlrange="-6.28 6.28"
-        forcerange="-87 87"
-      />
+      <material specular="0.5" shininess="0.25"/>
+      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
+      <general dyntype="none" biastype="affine" ctrlrange="-6.28 6.28" forcerange="-87 87"/>
       <default class="infiniteJoints">
-        <joint limited="false" />
+        <joint limited="false"/>
       </default>
       <default class="limitedJoints">
-        <joint limited="true" />
+        <joint limited="true"/>
       </default>
-      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4" />
+      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
     </default>
     <!-- robotiq 2f85 defaults -->
     <default class="2f85">
-      <mesh scale="0.001 0.001 0.001" />
-      <general biastype="affine" />
+      <mesh scale="0.001 0.001 0.001"/>
+      <general biastype="affine"/>
 
-      <joint axis="1 0 0" />
+      <joint axis="1 0 0"/>
       <default class="knuckle_link">
-        <joint
-          range="0 0.8"
-          armature="0.005"
-          damping="0.1"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="0 0.8" armature="0.005" damping="0.1" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
       <default class="finger_tip_link">
-        <joint
-          range="-0.872664 0.872664"
-          armature="0.001"
-          pos="0 -0.018 0.0065"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="-0.872664 0.872664" armature="0.001" pos="0 -0.018 0.0065" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
       <default class="inner_knuckle_link">
-        <joint
-          range="-0.29670597283 0.8"
-          armature="0.001"
-          stiffness="0.05"
-          springref="2.62"
-          damping="0.00125"
-        />
+        <joint range="-0.29670597283 0.8" armature="0.001" stiffness="0.05" springref="2.62" damping="0.00125"/>
       </default>
       <default class="finger_link">
-        <joint
-          range="-1.57 0"
-          armature="0.001"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="-1.57 0" armature="0.001" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
 
       <default class="visual">
-        <geom type="mesh" contype="0" conaffinity="0" group="2" />
+        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
       </default>
       <default class="collision">
-        <geom type="mesh" group="3" condim="6" />
+        <geom type="mesh" group="3" condim="6"/>
         <default class="pad_box1">
-          <geom
-            mass="0"
-            type="box"
-            pos="0 -0.0026 0.028125"
-            size="0.011 0.004 0.009375"
-            friction="0.7"
-            solimp="0.95 0.99 0.001"
-            solref="0.004 1"
-            priority="1"
-            rgba="0.55 0.55 0.55 1"
-          />
+          <geom mass="0" type="box" pos="0 -0.0026 0.028125" size="0.011 0.004 0.009375" friction="0.7" solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.55 0.55 0.55 1"/>
         </default>
         <default class="pad_box2">
-          <geom
-            mass="0"
-            type="box"
-            pos="0 -0.0026 0.009375"
-            size="0.011 0.004 0.009375"
-            friction="0.6"
-            solimp="0.95 0.99 0.001"
-            solref="0.004 1"
-            priority="1"
-            rgba="0.45 0.45 0.45 1"
-          />
+          <geom mass="0" type="box" pos="0 -0.0026 0.009375" size="0.011 0.004 0.009375" friction="0.6" solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.45 0.45 0.45 1"/>
         </default>
       </default>
     </default>
   </default>
 
   <asset>
-    <material name="metal" rgba="0.58 0.58 0.58 1" />
-    <material name="silicone" rgba="0.1882 0.1882 0.1882 1" />
-    <material name="white" rgba="1.0 1.0 1.0 1" />
-    <material name="gray" rgba="0.4627 0.4627 0.4627 1" />
-    <material name="black" rgba="0.149 0.149 0.149 1" />
+    <material name="metal" rgba="0.58 0.58 0.58 1"/>
+    <material name="silicone" rgba="0.1882 0.1882 0.1882 1"/>
+    <material name="white" rgba="1.0 1.0 1.0 1"/>
+    <material name="gray" rgba="0.4627 0.4627 0.4627 1"/>
+    <material name="black" rgba="0.149 0.149 0.149 1"/>
 
     <!-- Kinova collision meshes -->
-    <mesh name="base_link_c" file="kinova/base_link.STL" />
-    <mesh name="shoulder_link_c" file="kinova/shoulder_link.STL" />
-    <mesh name="half_arm_1_link_c" file="kinova/half_arm_1_link.STL" />
-    <mesh name="half_arm_2_link_c" file="kinova/half_arm_2_link.STL" />
-    <mesh name="forearm_link_c" file="kinova/forearm_link.STL" />
-    <mesh
-      name="spherical_wrist_1_link_c"
-      file="kinova/spherical_wrist_1_link.STL"
-    />
-    <mesh
-      name="spherical_wrist_2_link_c"
-      file="kinova/spherical_wrist_2_link.STL"
-    />
-    <mesh
-      name="bracelet_with_vision_link_c"
-      file="kinova/bracelet_with_vision_link.STL"
-    />
+    <mesh name="base_link_c" file="kinova/base_link.STL"/>
+    <mesh name="shoulder_link_c" file="kinova/shoulder_link.STL"/>
+    <mesh name="half_arm_1_link_c" file="kinova/half_arm_1_link.STL"/>
+    <mesh name="half_arm_2_link_c" file="kinova/half_arm_2_link.STL"/>
+    <mesh name="forearm_link_c" file="kinova/forearm_link.STL"/>
+    <mesh name="spherical_wrist_1_link_c" file="kinova/spherical_wrist_1_link.STL"/>
+    <mesh name="spherical_wrist_2_link_c" file="kinova/spherical_wrist_2_link.STL"/>
+    <mesh name="bracelet_with_vision_link_c" file="kinova/bracelet_with_vision_link.STL"/>
     <!-- Kinova visual meshes -->
-    <mesh name="base_link" file="kinova/base_link.obj" />
-    <mesh name="shoulder_link" file="kinova/shoulder_link.obj" />
-    <mesh name="half_arm_1_link" file="kinova/half_arm_1_link.obj" />
-    <mesh name="half_arm_2_link" file="kinova/half_arm_2_link.obj" />
-    <mesh name="forearm_link" file="kinova/forearm_link.obj" />
-    <mesh
-      name="spherical_wrist_1_link"
-      file="kinova/spherical_wrist_1_link.obj"
-    />
-    <mesh
-      name="spherical_wrist_2_link"
-      file="kinova/spherical_wrist_2_link.obj"
-    />
-    <mesh
-      name="bracelet_with_vision_link"
-      file="kinova/bracelet_with_vision_link.obj"
-    />
+    <mesh name="base_link" file="kinova/base_link.obj"/>
+    <mesh name="shoulder_link" file="kinova/shoulder_link.obj"/>
+    <mesh name="half_arm_1_link" file="kinova/half_arm_1_link.obj"/>
+    <mesh name="half_arm_2_link" file="kinova/half_arm_2_link.obj"/>
+    <mesh name="forearm_link" file="kinova/forearm_link.obj"/>
+    <mesh name="spherical_wrist_1_link" file="kinova/spherical_wrist_1_link.obj"/>
+    <mesh name="spherical_wrist_2_link" file="kinova/spherical_wrist_2_link.obj"/>
+    <mesh name="bracelet_with_vision_link" file="kinova/bracelet_with_vision_link.obj"/>
     <!-- Robotiq 2f85 meshes -->
-    <mesh name="robotiq_85_base" class="2f85" file="robotiq_2f85/base.stl" />
-    <mesh
-      name="robotiq_85_knuckle_link"
-      class="2f85"
-      file="robotiq_2f85/knuckle_link.stl"
-    />
-    <mesh
-      name="robotiq_85_inner_knuckle_link"
-      class="2f85"
-      file="robotiq_2f85/inner_knuckle_link.stl"
-    />
-    <mesh
-      name="robotiq_85_finger_link"
-      class="2f85"
-      file="robotiq_2f85/finger_link.stl"
-    />
-    <mesh
-      name="robotiq_85_finger_tip_link"
-      class="2f85"
-      file="robotiq_2f85/finger_tip_link.stl"
-    />
-    <mesh name="robotiq_85_pad" class="2f85" file="robotiq_2f85/pad.stl" />
-    <mesh
-      name="robotiq_85_silicone_pad"
-      class="2f85"
-      file="robotiq_2f85/silicone_pad.stl"
-    />
-    <mesh name="rafti_finger" file="rafti_finger.obj" />
-    <mesh name="rafti_finger_collision_1" file="rafti_finger_collision_1.obj" />
-    <mesh name="rafti_finger_collision_2" file="rafti_finger_collision_2.obj" />
-    <mesh name="rafti_finger_collision_3" file="rafti_finger_collision_3.obj" />
-    <mesh name="rafti_finger_collision_4" file="rafti_finger_collision_4.obj" />
+    <mesh name="robotiq_85_base" class="2f85" file="robotiq_2f85/base.stl"/>
+    <mesh name="robotiq_85_knuckle_link" class="2f85" file="robotiq_2f85/knuckle_link.stl"/>
+    <mesh name="robotiq_85_inner_knuckle_link" class="2f85" file="robotiq_2f85/inner_knuckle_link.stl"/>
+    <mesh name="robotiq_85_finger_link" class="2f85" file="robotiq_2f85/finger_link.stl"/>
+    <mesh name="robotiq_85_finger_tip_link" class="2f85" file="robotiq_2f85/finger_tip_link.stl"/>
+    <mesh name="robotiq_85_pad" class="2f85" file="robotiq_2f85/pad.stl"/>
+    <mesh name="robotiq_85_silicone_pad" class="2f85" file="robotiq_2f85/silicone_pad.stl"/>
+    <mesh name="rafti_finger" file="rafti_finger.obj"/>
+    <mesh name="rafti_finger_collision_1" file="rafti_finger_collision_1.obj"/>
+    <mesh name="rafti_finger_collision_2" file="rafti_finger_collision_2.obj"/>
+    <mesh name="rafti_finger_collision_3" file="rafti_finger_collision_3.obj"/>
+    <mesh name="rafti_finger_collision_4" file="rafti_finger_collision_4.obj"/>
   </asset>
 
   <contact>
-    <exclude body1="base_link" body2="shoulder_link" />
+    <exclude body1="base_link" body2="shoulder_link"/>
   </contact>
 
   <worldbody>
-    <light name="top" pos="0 0 2" mode="trackcom" />
+    <light name="top" pos="0 0 2" mode="trackcom"/>
 
     <body name="base_link" gravcomp="1" pos="0 0 0.02" quat="1 0 0 0">
-      <inertial
-        pos="-0.000648 -0.000166 0.084487"
-        quat="1 0 0 0"
-        mass="1.697"
-        diaginertia="0.004622 0.004495 0.002079"
-      />
-      <geom class="visual" mesh="base_link" material="white" />
-      <geom class="collision" mesh="base_link_c" />
-      <body
-        name="shoulder_link"
-        gravcomp="1"
-        pos="0 0 0.15643"
-        quat="-3.67321e-06 1 0 0"
-      >
-        <inertial
-          pos="-2.3e-05 -0.010364 -0.07336"
-          quat="0.707051 0.0451246 -0.0453544 0.704263"
-          mass="1.3773"
-          diaginertia="0.00488868 0.00457 0.00135132"
-        />
-        <joint class="infiniteJoints" name="joint_1" armature="0.0000015" />
-        <geom class="visual" mesh="shoulder_link" material="white" />
-        <geom class="collision" mesh="shoulder_link_c" />
-        <body
-          name="half_arm_1_link"
-          gravcomp="1"
-          pos="0 0.005375 -0.12838"
-          quat="0.707105 0.707108 0 0"
-        >
-          <inertial
-            pos="-4.4e-05 -0.09958 -0.013278"
-            quat="0.482348 0.516286 -0.516862 0.483366"
-            mass="1.1636"
-            diaginertia="0.0113017 0.011088 0.00102532"
-          />
-          <joint
-            class="limitedJoints"
-            name="joint_2"
-            range="-2.41 2.41"
-            armature="0.0000015"
-          />
-          <geom class="visual" mesh="half_arm_1_link" material="white" />
-          <geom class="collision" mesh="half_arm_1_link_c" />
-          <body
-            name="half_arm_2_link"
-            gravcomp="1"
-            pos="0 -0.21038 -0.006375"
-            quat="0.707105 -0.707108 0 0"
-          >
-            <inertial
-              pos="-4.4e-05 -0.006641 -0.117892"
-              quat="0.706144 0.0213722 -0.0209128 0.707437"
-              mass="1.1636"
-              diaginertia="0.0111633 0.010932 0.00100671"
-            />
-            <joint class="infiniteJoints" name="joint_3" armature="0.0000015" />
-            <geom class="visual" mesh="half_arm_2_link" material="white" />
-            <geom class="collision" mesh="half_arm_2_link_c" />
-            <body
-              name="forearm_link"
-              gravcomp="1"
-              pos="0 0.006375 -0.21038"
-              quat="0.707105 0.707108 0 0"
-            >
-              <inertial
-                pos="-1.8e-05 -0.075478 -0.015006"
-                quat="0.483678 0.515961 -0.515859 0.483455"
-                mass="0.9302"
-                diaginertia="0.00834839 0.008147 0.000598606"
-              />
-              <joint
-                class="limitedJoints"
-                name="joint_4"
-                range="-2.66 2.66"
-                armature="0.0000015"
-              />
-              <geom class="visual" mesh="forearm_link" material="white" />
-              <geom class="collision" mesh="forearm_link_c" />
-              <body
-                name="spherical_wrist_1_link"
-                gravcomp="1"
-                pos="0 -0.20843 -0.006375"
-                quat="0.707105 -0.707108 0 0"
-              >
-                <inertial
-                  pos="1e-06 -0.009432 -0.063883"
-                  quat="0.703558 0.0707492 -0.0707492 0.703558"
-                  mass="0.6781"
-                  diaginertia="0.00165901 0.001596 0.000346988"
-                />
-                <joint
-                  class="infiniteJoints"
-                  name="joint_5"
-                  armature="0.000001928"
-                />
-                <geom
-                  class="visual"
-                  mesh="spherical_wrist_1_link"
-                  material="white"
-                />
-                <geom class="collision" mesh="spherical_wrist_1_link_c" />
-                <body
-                  name="spherical_wrist_2_link"
-                  gravcomp="1"
-                  pos="0 0.00017505 -0.10593"
-                  quat="0.707105 0.707108 0 0"
-                >
-                  <inertial
-                    pos="1e-06 -0.045483 -0.00965"
-                    quat="0.44426 0.550121 -0.550121 0.44426"
-                    mass="0.6781"
-                    diaginertia="0.00170087 0.001641 0.00035013"
-                  />
-                  <joint
-                    class="limitedJoints"
-                    name="joint_6"
-                    range="-2.23 2.23"
-                    armature="0.000001928"
-                  />
-                  <geom
-                    class="visual"
-                    mesh="spherical_wrist_2_link"
-                    material="white"
-                  />
-                  <geom class="collision" mesh="spherical_wrist_2_link_c" />
-                  <body
-                    name="bracelet_link"
-                    gravcomp="1"
-                    pos="0 -0.10593 -0.00017505"
-                    quat="0.707105 -0.707108 0 0"
-                  >
-                    <camera
-                      name="wrist_camera"
-                      pos="0.0 -0.056 -0.058"
-                      fovy="58"
-                      mode="fixed"
-                      resolution="640 480"
-                      quat="0.0 0.0 0.0 1.0"
-                    />
-                    <site
-                      name="wrist_camera_optical_frame"
-                      pos="0.0 -0.056 -0.058"
-                      quat="0.0 0.0 1.0 0.0"
-                    />
-                    <inertial
-                      pos="-9.3e-05 0.000132 -0.022905"
-                      quat="-0.0368025 0.719402 -0.0444884 0.69219"
-                      mass="0.364"
-                      diaginertia="0.00024027 0.000222769 0.000213961"
-                    />
-                    <joint
-                      class="infiniteJoints"
-                      name="joint_7"
-                      armature="0.000001928"
-                    />
-                    <geom
-                      class="visual"
-                      mesh="bracelet_with_vision_link"
-                      material="white"
-                    />
-                    <geom
-                      class="collision"
-                      mesh="bracelet_with_vision_link_c"
-                    />
+      <inertial pos="-0.000648 -0.000166 0.084487" quat="1 0 0 0" mass="1.697" diaginertia="0.004622 0.004495 0.002079"/>
+      <geom class="visual" mesh="base_link" material="white"/>
+      <geom class="collision" mesh="base_link_c"/>
+      <body name="shoulder_link" gravcomp="1" pos="0 0 0.15643" quat="-3.67321e-06 1 0 0">
+        <inertial pos="-2.3e-05 -0.010364 -0.07336" quat="0.707051 0.0451246 -0.0453544 0.704263" mass="1.3773" diaginertia="0.00488868 0.00457 0.00135132"/>
+        <joint class="infiniteJoints" name="joint_1" armature="0.0000015"/>
+        <geom class="visual" mesh="shoulder_link" material="white"/>
+        <geom class="collision" mesh="shoulder_link_c"/>
+        <body name="half_arm_1_link" gravcomp="1" pos="0 0.005375 -0.12838" quat="0.707105 0.707108 0 0">
+          <inertial pos="-4.4e-05 -0.09958 -0.013278" quat="0.482348 0.516286 -0.516862 0.483366" mass="1.1636" diaginertia="0.0113017 0.011088 0.00102532"/>
+          <joint class="limitedJoints" name="joint_2" range="-2.41 2.41" armature="0.0000015"/>
+          <geom class="visual" mesh="half_arm_1_link" material="white"/>
+          <geom class="collision" mesh="half_arm_1_link_c"/>
+          <body name="half_arm_2_link" gravcomp="1" pos="0 -0.21038 -0.006375" quat="0.707105 -0.707108 0 0">
+            <inertial pos="-4.4e-05 -0.006641 -0.117892" quat="0.706144 0.0213722 -0.0209128 0.707437" mass="1.1636" diaginertia="0.0111633 0.010932 0.00100671"/>
+            <joint class="infiniteJoints" name="joint_3" armature="0.0000015"/>
+            <geom class="visual" mesh="half_arm_2_link" material="white"/>
+            <geom class="collision" mesh="half_arm_2_link_c"/>
+            <body name="forearm_link" gravcomp="1" pos="0 0.006375 -0.21038" quat="0.707105 0.707108 0 0">
+              <inertial pos="-1.8e-05 -0.075478 -0.015006" quat="0.483678 0.515961 -0.515859 0.483455" mass="0.9302" diaginertia="0.00834839 0.008147 0.000598606"/>
+              <joint class="limitedJoints" name="joint_4" range="-2.66 2.66" armature="0.0000015"/>
+              <geom class="visual" mesh="forearm_link" material="white"/>
+              <geom class="collision" mesh="forearm_link_c"/>
+              <body name="spherical_wrist_1_link" gravcomp="1" pos="0 -0.20843 -0.006375" quat="0.707105 -0.707108 0 0">
+                <inertial pos="1e-06 -0.009432 -0.063883" quat="0.703558 0.0707492 -0.0707492 0.703558" mass="0.6781" diaginertia="0.00165901 0.001596 0.000346988"/>
+                <joint class="infiniteJoints" name="joint_5" armature="0.000001928"/>
+                <geom class="visual" mesh="spherical_wrist_1_link" material="white"/>
+                <geom class="collision" mesh="spherical_wrist_1_link_c"/>
+                <body name="spherical_wrist_2_link" gravcomp="1" pos="0 0.00017505 -0.10593" quat="0.707105 0.707108 0 0">
+                  <inertial pos="1e-06 -0.045483 -0.00965" quat="0.44426 0.550121 -0.550121 0.44426" mass="0.6781" diaginertia="0.00170087 0.001641 0.00035013"/>
+                  <joint class="limitedJoints" name="joint_6" range="-2.23 2.23" armature="0.000001928"/>
+                  <geom class="visual" mesh="spherical_wrist_2_link" material="white"/>
+                  <geom class="collision" mesh="spherical_wrist_2_link_c"/>
+                  <body name="bracelet_link" gravcomp="1" pos="0 -0.10593 -0.00017505" quat="0.707105 -0.707108 0 0">
+                    <camera name="wrist_camera" pos="0.0 -0.056 -0.058" fovy="58" mode="fixed" resolution="640 480" quat="0.0 0.0 0.0 1.0"/>
+                    <site name="wrist_camera_optical_frame" pos="0.0 -0.056 -0.058" quat="0.0 0.0 1.0 0.0"/>
+                    <inertial pos="-9.3e-05 0.000132 -0.022905" quat="-0.0368025 0.719402 -0.0444884 0.69219" mass="0.364" diaginertia="0.00024027 0.000222769 0.000213961"/>
+                    <joint class="infiniteJoints" name="joint_7" armature="0.000001928"/>
+                    <geom class="visual" mesh="bracelet_with_vision_link" material="white"/>
+                    <geom class="collision" mesh="bracelet_with_vision_link_c"/>
                     <body childclass="2f85">
                       <!-- site for end effector's f/t sensor right at the base of the robotiq gripper -->
-                      <site
-                        name="fts"
-                        size="0.01"
-                        pos="0.0 0.0 -0.0615"
-                        quat="0 0.0 1.0 0.0"
-                      />
+                      <site name="fts" size="0.01" pos="0.0 0.0 -0.0615" quat="0 0.0 1.0 0.0"/>
                       <!-- Right-hand side 4-bar linkage -->
-                      <body
-                        name="robotiq_85_base_link"
-                        gravcomp="1"
-                        pos="0 0 -0.0615"
-                        quat="0 -0.707 0.707 0"
-                      >
-                        <inertial
-                          mass="0.777441"
-                          pos="0 -2.70394e-05 0.0354675"
-                          quat="1 -0.00152849 0 0"
-                          diaginertia="0.000260285 0.000225381 0.000152708"
-                        />
-                        <geom
-                          class="visual"
-                          mesh="robotiq_85_base"
-                          material="black"
-                        />
-                        <geom class="collision" mesh="robotiq_85_base" />
-                        <site
-                          name="pinch"
-                          pos="0 0 0.145"
-                          type="sphere"
-                          group="5"
-                          rgba="0.9 0.9 0.9 1"
-                          size="0.005"
-                        />
+                      <body name="robotiq_85_base_link" gravcomp="1" pos="0 0 -0.0615" quat="0 -0.707 0.707 0">
+                        <inertial mass="0.777441" pos="0 -2.70394e-05 0.0354675" quat="1 -0.00152849 0 0" diaginertia="0.000260285 0.000225381 0.000152708"/>
+                        <geom class="visual" mesh="robotiq_85_base" material="black"/>
+                        <geom class="collision" mesh="robotiq_85_base"/>
+                        <site name="pinch" pos="0 0 0.145" type="sphere" group="5" rgba="0.9 0.9 0.9 1" size="0.005"/>
                         <!-- Right-hand side 4-bar linkage -->
-                        <body
-                          name="robotiq_85_right_knuckle_link"
-                          gravcomp="1"
-                          pos="0 0.0306011 0.054904"
-                        >
-                          <inertial
-                            mass="0.00899563"
-                            pos="2.96931e-12 0.0177547 0.00107314"
-                            quat="0.681301 0.732003 0 0"
-                            diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"
-                          />
-                          <joint
-                            name="robotiq_85_right_knuckle_joint"
-                            class="knuckle_link"
-                          />
-                          <geom
-                            class="visual"
-                            mesh="robotiq_85_knuckle_link"
-                            material="gray"
-                          />
-                          <geom
-                            class="collision"
-                            mesh="robotiq_85_knuckle_link"
-                          />
-                          <body
-                            name="robotiq_85_right_finger_link"
-                            gravcomp="1"
-                            pos="0 0.0315 -0.0041"
-                          >
-                            <inertial
-                              mass="0.0140974"
-                              pos="0 0.00301209 0.0232175"
-                              quat="0.705636 -0.0455904 0.0455904 0.705636"
-                              diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"
-                            />
-                            <joint
-                              name="robotiq_85_right_finger_joint"
-                              class="finger_link"
-                            />
-                            <geom
-                              class="visual"
-                              mesh="robotiq_85_finger_link"
-                              material="black"
-                            />
-                            <geom
-                              class="collision"
-                              mesh="robotiq_85_finger_link"
-                            />
+                        <body name="robotiq_85_right_knuckle_link" gravcomp="1" pos="0 0.0306011 0.054904">
+                          <inertial mass="0.00899563" pos="2.96931e-12 0.0177547 0.00107314" quat="0.681301 0.732003 0 0" diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
+                          <joint name="robotiq_85_right_knuckle_joint" class="knuckle_link"/>
+                          <geom class="visual" mesh="robotiq_85_knuckle_link" material="gray"/>
+                          <geom class="collision" mesh="robotiq_85_knuckle_link"/>
+                          <body name="robotiq_85_right_finger_link" gravcomp="1" pos="0 0.0315 -0.0041">
+                            <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636" diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
+                            <joint name="robotiq_85_right_finger_joint" class="finger_link"/>
+                            <geom class="visual" mesh="robotiq_85_finger_link" material="black"/>
+                            <geom class="collision" mesh="robotiq_85_finger_link"/>
                           </body>
                         </body>
-                        <body
-                          name="robotiq_85_right_inner_knuckle_link"
-                          gravcomp="1"
-                          pos="0 0.0132 0.0609"
-                        >
-                          <inertial
-                            mass="0.0221642"
-                            pos="-8.65005e-09 0.0181624 0.0212658"
-                            quat="0.663403 -0.244737 0.244737 0.663403"
-                            diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"
-                          />
-                          <joint
-                            name="robotiq_85_right_inner_knuckle_joint"
-                            class="inner_knuckle_link"
-                          />
-                          <geom
-                            class="visual"
-                            mesh="robotiq_85_inner_knuckle_link"
-                            material="black"
-                          />
-                          <geom
-                            class="collision"
-                            mesh="robotiq_85_inner_knuckle_link"
-                          />
-                          <body
-                            name="robotiq_85_right_finger_tip_link"
-                            gravcomp="1"
-                            pos="0 0.055 0.0375"
-                          >
-                            <inertial
-                              mass="0.0125222"
-                              pos="0 -0.011046 0.0124786"
-                              quat="1 0.1664 0 0"
-                              diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"
-                            />
-                            <joint
-                              name="robotiq_85_right_finger_tip_joint"
-                              class="finger_tip_link"
-                            />
-                            <geom
-                              class="visual"
-                              mesh="robotiq_85_finger_tip_link"
-                              material="black"
-                            />
-                            <geom
-                              class="collision"
-                              mesh="robotiq_85_finger_tip_link"
-                            />
+                        <body name="robotiq_85_right_inner_knuckle_link" gravcomp="1" pos="0 0.0132 0.0609">
+                          <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403" diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+                          <joint name="robotiq_85_right_inner_knuckle_joint" class="inner_knuckle_link"/>
+                          <geom class="visual" mesh="robotiq_85_inner_knuckle_link" material="black"/>
+                          <geom class="collision" mesh="robotiq_85_inner_knuckle_link"/>
+                          <body name="robotiq_85_right_finger_tip_link" gravcomp="1" pos="0 0.055 0.0375">
+                            <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0" diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
+                            <joint name="robotiq_85_right_finger_tip_joint" class="finger_tip_link"/>
+                            <geom class="visual" mesh="robotiq_85_finger_tip_link" material="black"/>
+                            <geom class="collision" mesh="robotiq_85_finger_tip_link"/>
                             <body name="right_pad" pos="0 -0.0189 0.01352">
-                              <geom class="pad_box1" name="right_pad1" />
-                              <geom class="pad_box2" name="right_pad2" />
-                              <inertial
-                                mass="0.0035"
-                                pos="0 -0.0025 0.0185"
-                                quat="0.707107 0 0 0.707107"
-                                diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"
-                              />
-                              <geom class="visual" mesh="robotiq_85_pad" />
+                              <geom class="pad_box1" name="right_pad1"/>
+                              <geom class="pad_box2" name="right_pad2"/>
+                              <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="0.707107 0 0 0.707107" diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
+                              <geom class="visual" mesh="robotiq_85_pad"/>
                               <body name="right_silicone_pad">
-                                <geom
-                                  class="visual"
-                                  mesh="robotiq_85_silicone_pad"
-                                  material="black"
-                                />
+                                <geom class="visual" mesh="robotiq_85_silicone_pad" material="black"/>
                               </body>
                               <!--                              TODO uncomment to enable RAFTI fingers -->
-                              <body
-                                name="right_rafti_finger_link"
-                                pos="0 0 0.01"
-                                quat="0 0 0 1"
-                              >
-                                <inertial
-                                  mass="0.005"
-                                  pos="0 0 0"
-                                  diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"
-                                />
-                                <geom
-                                  class="visual"
-                                  mesh="rafti_finger"
-                                  material="black"
-                                />
-                                <geom
-                                  class="collision"
-                                  mesh="rafti_finger_collision_1"
-                                  condim="6"
-                                />
-                                <geom
-                                  class="collision"
-                                  mesh="rafti_finger_collision_2"
-                                  condim="6"
-                                />
-                                <geom
-                                  class="collision"
-                                  mesh="rafti_finger_collision_3"
-                                  condim="6"
-                                />
-                                <geom
-                                  class="collision"
-                                  mesh="rafti_finger_collision_4"
-                                  condim="6"
-                                />
+                              <body name="right_rafti_finger_link" pos="0 0 0.01" quat="0 0 0 1">
+                                <inertial mass="0.005" pos="0 0 0" diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+                                <geom class="visual" mesh="rafti_finger" material="black"/>
+                                <geom class="collision" mesh="rafti_finger_collision_1" condim="6"/>
+                                <geom class="collision" mesh="rafti_finger_collision_2" condim="6"/>
+                                <geom class="collision" mesh="rafti_finger_collision_3" condim="6"/>
+                                <geom class="collision" mesh="rafti_finger_collision_4" condim="6"/>
                               </body>
                             </body>
                           </body>
                         </body>
                         <!-- Left-hand side 4-bar linkage -->
-                        <body
-                          name="robotiq_85_left_knuckle_link"
-                          gravcomp="1"
-                          pos="0 -0.0306011 0.054904"
-                          quat="0 0 0 1"
-                        >
-                          <inertial
-                            mass="0.00899563"
-                            pos="0 0.0177547 0.00107314"
-                            quat="0.681301 0.732003 0 0"
-                            diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"
-                          />
-                          <joint
-                            name="robotiq_85_left_knuckle_joint"
-                            class="knuckle_link"
-                          />
-                          <geom
-                            class="visual"
-                            mesh="robotiq_85_knuckle_link"
-                            material="gray"
-                          />
-                          <geom
-                            class="collision"
-                            mesh="robotiq_85_knuckle_link"
-                          />
-                          <body
-                            name="robotiq_85_left_finger_link"
-                            gravcomp="1"
-                            pos="0 0.0315 -0.0041"
-                          >
-                            <inertial
-                              mass="0.0140974"
-                              pos="0 0.00301209 0.0232175"
-                              quat="0.705636 -0.0455904 0.0455904 0.705636"
-                              diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"
-                            />
-                            <joint
-                              name="robotiq_85_left_finger_joint"
-                              class="finger_link"
-                            />
-                            <geom
-                              class="visual"
-                              mesh="robotiq_85_finger_link"
-                              material="black"
-                            />
-                            <geom
-                              class="collision"
-                              mesh="robotiq_85_finger_link"
-                            />
+                        <body name="robotiq_85_left_knuckle_link" gravcomp="1" pos="0 -0.0306011 0.054904" quat="0 0 0 1">
+                          <inertial mass="0.00899563" pos="0 0.0177547 0.00107314" quat="0.681301 0.732003 0 0" diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
+                          <joint name="robotiq_85_left_knuckle_joint" class="knuckle_link"/>
+                          <geom class="visual" mesh="robotiq_85_knuckle_link" material="gray"/>
+                          <geom class="collision" mesh="robotiq_85_knuckle_link"/>
+                          <body name="robotiq_85_left_finger_link" gravcomp="1" pos="0 0.0315 -0.0041">
+                            <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636" diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
+                            <joint name="robotiq_85_left_finger_joint" class="finger_link"/>
+                            <geom class="visual" mesh="robotiq_85_finger_link" material="black"/>
+                            <geom class="collision" mesh="robotiq_85_finger_link"/>
                           </body>
                         </body>
-                        <body
-                          name="robotiq_85_left_inner_knuckle_link"
-                          gravcomp="1"
-                          pos="0 -0.0132 0.0609"
-                          quat="0 0 0 1"
-                        >
-                          <inertial
-                            mass="0.0221642"
-                            pos="-8.65005e-09 0.0181624 0.0212658"
-                            quat="0.663403 -0.244737 0.244737 0.663403"
-                            diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"
-                          />
-                          <joint
-                            name="robotiq_85_left_inner_knuckle_joint"
-                            class="inner_knuckle_link"
-                          />
-                          <geom
-                            class="visual"
-                            mesh="robotiq_85_inner_knuckle_link"
-                            material="black"
-                          />
-                          <geom
-                            class="collision"
-                            mesh="robotiq_85_inner_knuckle_link"
-                          />
-                          <body
-                            name="robotiq_85_left_finger_tip_link"
-                            gravcomp="1"
-                            pos="0 0.055 0.0375"
-                          >
-                            <inertial
-                              mass="0.0125222"
-                              pos="0 -0.011046 0.0124786"
-                              quat="1 0.1664 0 0"
-                              diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"
-                            />
-                            <joint
-                              name="robotiq_85_left_finger_tip_joint"
-                              class="finger_tip_link"
-                            />
-                            <geom
-                              class="visual"
-                              mesh="robotiq_85_finger_tip_link"
-                              material="black"
-                            />
-                            <geom
-                              class="collision"
-                              mesh="robotiq_85_finger_tip_link"
-                            />
+                        <body name="robotiq_85_left_inner_knuckle_link" gravcomp="1" pos="0 -0.0132 0.0609" quat="0 0 0 1">
+                          <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403" diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+                          <joint name="robotiq_85_left_inner_knuckle_joint" class="inner_knuckle_link"/>
+                          <geom class="visual" mesh="robotiq_85_inner_knuckle_link" material="black"/>
+                          <geom class="collision" mesh="robotiq_85_inner_knuckle_link"/>
+                          <body name="robotiq_85_left_finger_tip_link" gravcomp="1" pos="0 0.055 0.0375">
+                            <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0" diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
+                            <joint name="robotiq_85_left_finger_tip_joint" class="finger_tip_link"/>
+                            <geom class="visual" mesh="robotiq_85_finger_tip_link" material="black"/>
+                            <geom class="collision" mesh="robotiq_85_finger_tip_link"/>
                             <body name="left_pad" pos="0 -0.0189 0.01352">
-                              <geom class="pad_box1" name="left_pad1" />
-                              <geom class="pad_box2" name="left_pad2" />
-                              <inertial
-                                mass="0.0035"
-                                pos="0 -0.0025 0.0185"
-                                quat="1 0 0 1"
-                                diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"
-                              />
-                              <geom class="visual" mesh="robotiq_85_pad" />
+                              <geom class="pad_box1" name="left_pad1"/>
+                              <geom class="pad_box2" name="left_pad2"/>
+                              <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="1 0 0 1" diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
+                              <geom class="visual" mesh="robotiq_85_pad"/>
                               <body name="left_silicone_pad">
-                                <geom
-                                  class="visual"
-                                  mesh="robotiq_85_silicone_pad"
-                                  material="black"
-                                />
+                                <geom class="visual" mesh="robotiq_85_silicone_pad" material="black"/>
                               </body>
                               <!--                              TODO uncomment to enable RAFTI fingers -->
-                              <body
-                                name="left_rafti_finger_link"
-                                pos="0 0 0.01"
-                                quat="0 0 0 1"
-                              >
-                                <inertial mass="0.005" pos="0 0 0" />
-                                <geom
-                                  class="visual"
-                                  mesh="rafti_finger"
-                                  material="black"
-                                />
-                                <geom
-                                  class="collision"
-                                  mesh="rafti_finger_collision_1"
-                                  condim="6"
-                                />
-                                <geom
-                                  class="collision"
-                                  mesh="rafti_finger_collision_2"
-                                  condim="6"
-                                />
-                                <geom
-                                  class="collision"
-                                  mesh="rafti_finger_collision_3"
-                                  condim="6"
-                                />
-                                <geom
-                                  class="collision"
-                                  mesh="rafti_finger_collision_4"
-                                  condim="6"
-                                />
+                              <body name="left_rafti_finger_link" pos="0 0 0.01" quat="0 0 0 1">
+                                <inertial mass="0.005" pos="0 0 0"/>
+                                <geom class="visual" mesh="rafti_finger" material="black"/>
+                                <geom class="collision" mesh="rafti_finger_collision_1" condim="6"/>
+                                <geom class="collision" mesh="rafti_finger_collision_2" condim="6"/>
+                                <geom class="collision" mesh="rafti_finger_collision_3" condim="6"/>
+                                <geom class="collision" mesh="rafti_finger_collision_4" condim="6"/>
                               </body>
                             </body>
                           </body>
@@ -706,66 +247,30 @@
   </worldbody>
 
   <actuator>
-    <position class="kinova" name="joint_1" joint="joint_1" kp="800" kv="260" />
-    <position class="kinova" name="joint_2" joint="joint_2" kp="800" kv="260" />
-    <position class="kinova" name="joint_3" joint="joint_3" kp="260" kv="86" />
-    <position class="kinova" name="joint_4" joint="joint_4" kp="260" kv="86" />
-    <position class="kinova" name="joint_5" joint="joint_5" kp="130" kv="45" />
-    <position class="kinova" name="joint_6" joint="joint_6" kp="130" kv="45" />
-    <position class="kinova" name="joint_7" joint="joint_7" kp="130" kv="45" />
+    <position class="kinova" name="joint_1" joint="joint_1" kp="800" kv="260"/>
+    <position class="kinova" name="joint_2" joint="joint_2" kp="800" kv="260"/>
+    <position class="kinova" name="joint_3" joint="joint_3" kp="260" kv="86"/>
+    <position class="kinova" name="joint_4" joint="joint_4" kp="260" kv="86"/>
+    <position class="kinova" name="joint_5" joint="joint_5" kp="130" kv="45"/>
+    <position class="kinova" name="joint_6" joint="joint_6" kp="130" kv="45"/>
+    <position class="kinova" name="joint_7" joint="joint_7" kp="130" kv="45"/>
   </actuator>
 
   <!-- Robotiq gripper contact -->
   <contact>
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_left_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_right_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_left_inner_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_right_inner_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_right_finger_link"
-      body2="robotiq_85_right_finger_tip_link"
-    />
-    <exclude
-      body1="robotiq_85_left_finger_link"
-      body2="robotiq_85_left_finger_tip_link"
-    />
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_left_knuckle_link"/>
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_right_knuckle_link"/>
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_left_inner_knuckle_link"/>
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_right_inner_knuckle_link"/>
+    <exclude body1="robotiq_85_right_finger_link" body2="robotiq_85_right_finger_tip_link"/>
+    <exclude body1="robotiq_85_left_finger_link" body2="robotiq_85_left_finger_tip_link"/>
     <!--                              TODO uncomment to enable RAFTI fingers -->
-    <exclude
-      body1="right_rafti_finger_link"
-      body2="robotiq_85_right_finger_tip_link"
-    />
-    <exclude
-      body1="right_rafti_finger_link"
-      body2="robotiq_85_right_inner_knuckle_link"
-    />
-    <exclude
-      body1="right_rafti_finger_link"
-      body2="robotiq_85_right_knuckle_link"
-    />
-    <exclude
-      body1="left_rafti_finger_link"
-      body2="robotiq_85_left_finger_tip_link"
-    />
-    <exclude
-      body1="left_rafti_finger_link"
-      body2="robotiq_85_left_inner_knuckle_link"
-    />
-    <exclude
-      body1="left_rafti_finger_link"
-      body2="robotiq_85_left_knuckle_link"
-    />
+    <exclude body1="right_rafti_finger_link" body2="robotiq_85_right_finger_tip_link"/>
+    <exclude body1="right_rafti_finger_link" body2="robotiq_85_right_inner_knuckle_link"/>
+    <exclude body1="right_rafti_finger_link" body2="robotiq_85_right_knuckle_link"/>
+    <exclude body1="left_rafti_finger_link" body2="robotiq_85_left_finger_tip_link"/>
+    <exclude body1="left_rafti_finger_link" body2="robotiq_85_left_inner_knuckle_link"/>
+    <exclude body1="left_rafti_finger_link" body2="robotiq_85_left_knuckle_link"/>
   </contact>
 
   <!--
@@ -776,49 +281,23 @@
   -->
   <tendon>
     <fixed name="split">
-      <joint joint="robotiq_85_right_knuckle_joint" coef="0.5" />
-      <joint joint="robotiq_85_left_knuckle_joint" coef="0.5" />
+      <joint joint="robotiq_85_right_knuckle_joint" coef="0.5"/>
+      <joint joint="robotiq_85_left_knuckle_joint" coef="0.5"/>
     </fixed>
   </tendon>
 
   <equality>
-    <connect
-      anchor="0 0 0"
-      body1="robotiq_85_right_finger_tip_link"
-      body2="robotiq_85_right_finger_link"
-      solimp="0.95 0.99 0.001"
-      solref="0.005 1"
-    />
-    <connect
-      anchor="0 0 0"
-      body1="robotiq_85_left_finger_tip_link"
-      body2="robotiq_85_left_finger_link"
-      solimp="0.95 0.99 0.001"
-      solref="0.005 1"
-    />
-    <joint
-      joint1="robotiq_85_left_knuckle_joint"
-      joint2="robotiq_85_right_knuckle_joint"
-      polycoef="0 1 0 0 0"
-      solimp="0.95 0.99 0.001"
-      solref="0.005 1"
-    />
+    <connect anchor="0 0 0" body1="robotiq_85_right_finger_tip_link" body2="robotiq_85_right_finger_link" solimp="0.95 0.99 0.001" solref="0.005 1"/>
+    <connect anchor="0 0 0" body1="robotiq_85_left_finger_tip_link" body2="robotiq_85_left_finger_link" solimp="0.95 0.99 0.001" solref="0.005 1"/>
+    <joint joint1="robotiq_85_left_knuckle_joint" joint2="robotiq_85_right_knuckle_joint" polycoef="0 1 0 0 0" solimp="0.95 0.99 0.001" solref="0.005 1"/>
   </equality>
 
   <actuator>
-    <position
-      class="2f85"
-      name="robotiq_85_left_knuckle_joint"
-      tendon="split"
-      forcerange="-5 5"
-      ctrlrange="0 .8"
-      kp="100"
-      kv="100"
-    />
+    <position class="2f85" name="robotiq_85_left_knuckle_joint" tendon="split" forcerange="-5 5" ctrlrange="0 .8" kp="100" kv="100"/>
   </actuator>
 
   <sensor>
-    <force site="fts" />
-    <torque site="fts" />
+    <force site="fts"/>
+    <torque site="fts"/>
   </sensor>
 </mujoco>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/landsat_scene.xml b/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/landsat_scene.xml
index 987fcaf4..52f9f152 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/landsat_scene.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/landsat_scene.xml
@@ -1,132 +1,72 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="kinova scene">
-  <include file="gen3_7dof.xml" />
-  <option gravity="0 0 0" />
+  <include file="gen3_7dof.xml"/>
+  <option gravity="0 0 0"/>
   <option>
-    <flag multiccd="enable" />
+    <flag multiccd="enable"/>
   </option>
-  <option integrator="implicit" />
-  <statistic center="0.3 0 0.4" extent="1" />
+  <option integrator="implicit"/>
+  <statistic center="0.3 0 0.4" extent="1"/>
   <default>
-    <geom solref=".004 1" />
+    <geom solref=".004 1"/>
   </default>
 
   <visual>
-    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
-    <rgba haze="0.15 0.25 0.35 1" />
-    <global azimuth="120" elevation="-20" />
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
+    <rgba haze="0.15 0.25 0.35 1"/>
+    <global azimuth="120" elevation="-20"/>
   </visual>
 
   <asset>
-    <texture
-      type="2d"
-      name="april_tag"
-      file="assets/black_white.png"
-      width="32"
-      height="32"
-    />
-    <material name="april_tag" texture="april_tag" />
-    <texture
-      type="skybox"
-      file="assets/space.png"
-      gridsize="3 4"
-      gridlayout=".U..LFRB.D.."
-    />
-    <material name="landsat_gold" rgba="1.0 0.85 0.6  1" reflectance="0.2" />
-    <material name="metal_rafti" rgba="0.8 0.8 0.8 1" reflectance="0.5" />
+    <texture type="2d" name="april_tag" file="assets/black_white.png" width="32" height="32"/>
+    <material name="april_tag" texture="april_tag"/>
+    <texture type="skybox" file="assets/space.png" gridsize="3 4" gridlayout=".U..LFRB.D.."/>
+    <material name="landsat_gold" rgba="1.0 0.85 0.6  1" reflectance="0.2"/>
+    <material name="metal_rafti" rgba="0.8 0.8 0.8 1" reflectance="0.5"/>
   </asset>
 
   <asset>
-    <mesh name="landsat" file="landsat.obj" />
-    <mesh name="landsat_white" file="landsat_white.obj" />
-    <mesh name="landsat_glass" file="landsat_glass.obj" />
-    <mesh name="landsat_collision_1" file="landsat_collision_1.obj" />
-    <mesh name="landsat_collision_2" file="landsat_collision_2.obj" />
-    <mesh name="landsat_collision_3" file="landsat_collision_3.obj" />
-    <mesh name="landsat_collision_4" file="landsat_collision_4.obj" />
-    <mesh name="april_tag" file="april_tag.obj" />
-    <mesh name="rafti" file="rafti.obj" />
-    <mesh name="rafti_collision_1" file="rafti_collision_1.obj" />
-    <mesh name="rafti_collision_2" file="rafti_collision_2.obj" />
-    <mesh name="rafti_collision_3" file="rafti_collision_3.obj" />
-    <mesh name="rafti_collision_4" file="rafti_collision_4.obj" />
-    <mesh name="rafti_collision_5" file="rafti_collision_5.obj" />
+    <mesh name="landsat" file="landsat.obj"/>
+    <mesh name="landsat_white" file="landsat_white.obj"/>
+    <mesh name="landsat_glass" file="landsat_glass.obj"/>
+    <mesh name="landsat_collision_1" file="landsat_collision_1.obj"/>
+    <mesh name="landsat_collision_2" file="landsat_collision_2.obj"/>
+    <mesh name="landsat_collision_3" file="landsat_collision_3.obj"/>
+    <mesh name="landsat_collision_4" file="landsat_collision_4.obj"/>
+    <mesh name="april_tag" file="april_tag.obj"/>
+    <mesh name="rafti" file="rafti.obj"/>
+    <mesh name="rafti_collision_1" file="rafti_collision_1.obj"/>
+    <mesh name="rafti_collision_2" file="rafti_collision_2.obj"/>
+    <mesh name="rafti_collision_3" file="rafti_collision_3.obj"/>
+    <mesh name="rafti_collision_4" file="rafti_collision_4.obj"/>
+    <mesh name="rafti_collision_5" file="rafti_collision_5.obj"/>
   </asset>
 
   <worldbody>
-    <camera
-      name="scene_camera"
-      pos="-0.5 0.75 1.5"
-      fovy="75"
-      mode="fixed"
-      resolution="640 480"
-      quat="0.365715 0.232986 -0.484157 -0.759975"
-    />
-    <site
-      name="scene_camera_optical_frame"
-      pos="0.0 0.3 0.55"
-      quat="0.365715 0.232986 -0.484157 -0.759975"
-      size="0.03"
-    />
+    <camera name="scene_camera" pos="-0.5 0.75 1.5" fovy="75" mode="fixed" resolution="640 480" quat="0.365715 0.232986 -0.484157 -0.759975"/>
+    <site name="scene_camera_optical_frame" pos="0.0 0.3 0.55" quat="0.365715 0.232986 -0.484157 -0.759975" size="0.03"/>
   </worldbody>
 
   <worldbody>
     <body name="landsat" pos="1.5 1.75 1" quat="0 0 -0.707106781 -0.707106781">
-      <freejoint />
-      <geom
-        class="visual"
-        type="mesh"
-        material="landsat_gold"
-        mesh="landsat"
-        density="200"
-      />
-      <geom
-        class="visual"
-        type="mesh"
-        rgba="1.0 1.0 1.0 1"
-        mesh="landsat_white"
-        density="200"
-      />
-      <geom
-        class="visual"
-        type="mesh"
-        rgba="0.6 0.6 1.0 1"
-        mesh="landsat_glass"
-        density="200"
-      />
-      <geom
-        class="collision"
-        type="mesh"
-        mesh="landsat_collision_1"
-        density="1"
-      />
-      <geom
-        class="collision"
-        type="mesh"
-        mesh="landsat_collision_2"
-        density="1"
-      />
-      <geom
-        class="collision"
-        type="mesh"
-        mesh="landsat_collision_3"
-        density="1"
-      />
-      <geom
-        class="collision"
-        type="mesh"
-        mesh="landsat_collision_4"
-        density="1"
-      />
+      <freejoint/>
+      <geom class="visual" type="mesh" material="landsat_gold" mesh="landsat" density="200"/>
+      <geom class="visual" type="mesh" rgba="1.0 1.0 1.0 1" mesh="landsat_white" density="200"/>
+      <geom class="visual" type="mesh" rgba="0.6 0.6 1.0 1" mesh="landsat_glass" density="200"/>
+      <geom class="collision" type="mesh" mesh="landsat_collision_1" density="1"/>
+      <geom class="collision" type="mesh" mesh="landsat_collision_2" density="1"/>
+      <geom class="collision" type="mesh" mesh="landsat_collision_3" density="1"/>
+      <geom class="collision" type="mesh" mesh="landsat_collision_4" density="1"/>
       <body name="rafti" pos="0.565306 -0.3 -1.6" quat="0.5 0.5 0.5 0.5">
-        <geom class="visual" mesh="rafti" material="metal_rafti" />
-        <geom class="collision" mesh="rafti_collision_1" condim="6" />
-        <geom class="collision" mesh="rafti_collision_1" condim="6" />
-        <geom class="collision" mesh="rafti_collision_2" condim="6" />
-        <geom class="collision" mesh="rafti_collision_3" condim="6" />
-        <geom class="collision" mesh="rafti_collision_4" condim="6" />
-        <geom class="collision" mesh="rafti_collision_5" condim="6" />
+        <geom class="visual" mesh="rafti" material="metal_rafti"/>
+        <geom class="collision" mesh="rafti_collision_1" condim="6"/>
+        <geom class="collision" mesh="rafti_collision_1" condim="6"/>
+        <geom class="collision" mesh="rafti_collision_2" condim="6"/>
+        <geom class="collision" mesh="rafti_collision_3" condim="6"/>
+        <geom class="collision" mesh="rafti_collision_4" condim="6"/>
+        <geom class="collision" mesh="rafti_collision_5" condim="6"/>
         <body name="april_tag" pos="0 -.1 .132" quat="1 0 0 0">
-          <geom class="visual" mesh="april_tag" material="april_tag" />
+          <geom class="visual" mesh="april_tag" material="april_tag"/>
         </body>
       </body>
     </body>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/rafti_fingers_scene.xml b/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/rafti_fingers_scene.xml
index 80678631..c776ab24 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/rafti_fingers_scene.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/rafti_fingers_scene.xml
@@ -1,171 +1,120 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="kinova scene">
-  <include file="gen3_7dof_rafti_fingers.xml" />
-  <statistic center="0.3 0 0.4" extent="1" />
+  <include file="gen3_7dof_rafti_fingers.xml"/>
+  <statistic center="0.3 0 0.4" extent="1"/>
   <default>
-    <geom solref=".004 1" />
+    <geom solref=".004 1"/>
   </default>
 
   <visual>
-    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
-    <rgba haze="0.15 0.25 0.35 1" />
-    <global azimuth="120" elevation="-20" />
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
+    <rgba haze="0.15 0.25 0.35 1"/>
+    <global azimuth="120" elevation="-20"/>
   </visual>
 
   <asset>
-    <texture
-      type="skybox"
-      builtin="gradient"
-      rgb1="0.3 0.5 0.7"
-      rgb2="0 0 0"
-      width="512"
-      height="3072"
-    />
-    <texture
-      type="2d"
-      name="groundplane"
-      builtin="checker"
-      mark="edge"
-      rgb1="0.2 0.3 0.4"
-      rgb2="0.1 0.2 0.3"
-      markrgb="0.8 0.8 0.8"
-      width="300"
-      height="300"
-    />
-    <material
-      name="groundplane"
-      texture="groundplane"
-      texuniform="true"
-      texrepeat="5 5"
-      reflectance="0.2"
-    />
-    <texture
-      type="2d"
-      name="april_tag"
-      file="assets/black_white.png"
-      width="32"
-      height="32"
-    />
-    <material name="april_tag" texture="april_tag" />
-    <texture
-      type="2d"
-      name="satellite"
-      file="assets/satellite.png"
-      width="32"
-      height="32"
-    />
-    <material name="satellite" texture="satellite" />
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
+    <texture type="2d" name="april_tag" file="assets/black_white.png" width="32" height="32"/>
+    <material name="april_tag" texture="april_tag"/>
+    <texture type="2d" name="satellite" file="assets/satellite.png" width="32" height="32"/>
+    <material name="satellite" texture="satellite"/>
   </asset>
 
   <worldbody>
-    <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
-    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
-    <camera
-      name="scene_camera"
-      pos="0.0 0.3 0.55"
-      fovy="58"
-      mode="fixed"
-      resolution="640 480"
-      quat="0.365715 0.232986 -0.484157 -0.759975"
-    />
-    <site
-      name="scene_camera_optical_frame"
-      pos="0.0 0.3 0.55"
-      quat="0.365715 0.232986 -0.484157 -0.759975"
-      size="0.03"
-    />
+    <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
+    <camera name="scene_camera" pos="0.0 0.3 0.55" fovy="58" mode="fixed" resolution="640 480" quat="0.365715 0.232986 -0.484157 -0.759975"/>
+    <site name="scene_camera_optical_frame" pos="0.0 0.3 0.55" quat="0.365715 0.232986 -0.484157 -0.759975" size="0.03"/>
   </worldbody>
 
   <asset>
-    <mesh name="april_tag" file="april_tag.obj" />
-    <mesh name="satellite" file="satellite.obj" />
-    <mesh name="satellite_box" file="satellite_box.obj" />
-    <mesh name="rafti_collision_1" file="rafti_collision_1.obj" />
-    <mesh name="rafti_collision_2" file="rafti_collision_2.obj" />
-    <mesh name="rafti_collision_3" file="rafti_collision_3.obj" />
-    <mesh name="rafti_collision_4" file="rafti_collision_4.obj" />
-    <mesh name="rafti_collision_5" file="rafti_collision_5.obj" />
-    <mesh name="ring_collision_1" file="ring_collision_1.obj" />
-    <mesh name="ring_collision_2" file="ring_collision_2.obj" />
-    <mesh name="ring_collision_3" file="ring_collision_3.obj" />
-    <mesh name="ring_collision_4" file="ring_collision_4.obj" />
-    <mesh name="ring_collision_5" file="ring_collision_5.obj" />
-    <mesh name="ring_collision_6" file="ring_collision_6.obj" />
-    <mesh name="ring_collision_7" file="ring_collision_7.obj" />
-    <mesh name="ring_collision_8" file="ring_collision_8.obj" />
-    <mesh name="ring_collision_9" file="ring_collision_9.obj" />
-    <mesh name="ring_collision_10" file="ring_collision_10.obj" />
-    <mesh name="ring_collision_11" file="ring_collision_11.obj" />
-    <mesh name="ring_collision_12" file="ring_collision_12.obj" />
-    <mesh name="ring_collision_13" file="ring_collision_13.obj" />
-    <mesh name="ring_collision_14" file="ring_collision_14.obj" />
-    <mesh name="ring_collision_15" file="ring_collision_15.obj" />
-    <mesh name="ring_collision_16" file="ring_collision_16.obj" />
-    <mesh name="ring_collision_17" file="ring_collision_17.obj" />
-    <mesh name="ring_collision_18" file="ring_collision_18.obj" />
-    <mesh name="ring_collision_19" file="ring_collision_19.obj" />
-    <mesh name="ring_collision_20" file="ring_collision_20.obj" />
-    <mesh name="ring_collision_21" file="ring_collision_21.obj" />
-    <mesh name="ring_collision_22" file="ring_collision_22.obj" />
-    <mesh name="ring_collision_23" file="ring_collision_23.obj" />
-    <mesh name="ring_collision_24" file="ring_collision_24.obj" />
-    <mesh name="ring_collision_25" file="ring_collision_25.obj" />
-    <mesh name="ring_collision_26" file="ring_collision_26.obj" />
-    <mesh name="ring_collision_27" file="ring_collision_27.obj" />
-    <mesh name="ring_collision_28" file="ring_collision_28.obj" />
-    <mesh name="ring_collision_29" file="ring_collision_29.obj" />
-    <mesh name="ring_collision_30" file="ring_collision_30.obj" />
-    <mesh name="ring_collision_31" file="ring_collision_31.obj" />
-    <mesh name="ring_collision_32" file="ring_collision_32.obj" />
+    <mesh name="april_tag" file="april_tag.obj"/>
+    <mesh name="satellite" file="satellite.obj"/>
+    <mesh name="satellite_box" file="satellite_box.obj"/>
+    <mesh name="rafti_collision_1" file="rafti_collision_1.obj"/>
+    <mesh name="rafti_collision_2" file="rafti_collision_2.obj"/>
+    <mesh name="rafti_collision_3" file="rafti_collision_3.obj"/>
+    <mesh name="rafti_collision_4" file="rafti_collision_4.obj"/>
+    <mesh name="rafti_collision_5" file="rafti_collision_5.obj"/>
+    <mesh name="ring_collision_1" file="ring_collision_1.obj"/>
+    <mesh name="ring_collision_2" file="ring_collision_2.obj"/>
+    <mesh name="ring_collision_3" file="ring_collision_3.obj"/>
+    <mesh name="ring_collision_4" file="ring_collision_4.obj"/>
+    <mesh name="ring_collision_5" file="ring_collision_5.obj"/>
+    <mesh name="ring_collision_6" file="ring_collision_6.obj"/>
+    <mesh name="ring_collision_7" file="ring_collision_7.obj"/>
+    <mesh name="ring_collision_8" file="ring_collision_8.obj"/>
+    <mesh name="ring_collision_9" file="ring_collision_9.obj"/>
+    <mesh name="ring_collision_10" file="ring_collision_10.obj"/>
+    <mesh name="ring_collision_11" file="ring_collision_11.obj"/>
+    <mesh name="ring_collision_12" file="ring_collision_12.obj"/>
+    <mesh name="ring_collision_13" file="ring_collision_13.obj"/>
+    <mesh name="ring_collision_14" file="ring_collision_14.obj"/>
+    <mesh name="ring_collision_15" file="ring_collision_15.obj"/>
+    <mesh name="ring_collision_16" file="ring_collision_16.obj"/>
+    <mesh name="ring_collision_17" file="ring_collision_17.obj"/>
+    <mesh name="ring_collision_18" file="ring_collision_18.obj"/>
+    <mesh name="ring_collision_19" file="ring_collision_19.obj"/>
+    <mesh name="ring_collision_20" file="ring_collision_20.obj"/>
+    <mesh name="ring_collision_21" file="ring_collision_21.obj"/>
+    <mesh name="ring_collision_22" file="ring_collision_22.obj"/>
+    <mesh name="ring_collision_23" file="ring_collision_23.obj"/>
+    <mesh name="ring_collision_24" file="ring_collision_24.obj"/>
+    <mesh name="ring_collision_25" file="ring_collision_25.obj"/>
+    <mesh name="ring_collision_26" file="ring_collision_26.obj"/>
+    <mesh name="ring_collision_27" file="ring_collision_27.obj"/>
+    <mesh name="ring_collision_28" file="ring_collision_28.obj"/>
+    <mesh name="ring_collision_29" file="ring_collision_29.obj"/>
+    <mesh name="ring_collision_30" file="ring_collision_30.obj"/>
+    <mesh name="ring_collision_31" file="ring_collision_31.obj"/>
+    <mesh name="ring_collision_32" file="ring_collision_32.obj"/>
   </asset>
 
   <worldbody>
     <body name="satellite" pos=".43 -0.2 .133" quat="0.584012 0.0 0.0 0.811745">
-      <freejoint />
-      <inertial
-        pos="0.0 0.0 0.0"
-        quat="1 0 0 0"
-        mass="1.5"
-        diaginertia="0.00001 0.00001 0.00001"
-      />
-      <geom class="visual" mesh="satellite" material="satellite" />
-      <geom class="collision" mesh="satellite_box" condim="6" />
-      <geom class="collision" mesh="rafti_collision_1" condim="6" />
-      <geom class="collision" mesh="rafti_collision_2" condim="6" />
-      <geom class="collision" mesh="rafti_collision_3" condim="6" />
-      <geom class="collision" mesh="rafti_collision_4" condim="6" />
-      <geom class="collision" mesh="rafti_collision_5" condim="6" />
-      <geom class="collision" mesh="ring_collision_1" />
-      <geom class="collision" mesh="ring_collision_2" />
-      <geom class="collision" mesh="ring_collision_3" />
-      <geom class="collision" mesh="ring_collision_4" />
-      <geom class="collision" mesh="ring_collision_5" />
-      <geom class="collision" mesh="ring_collision_6" />
-      <geom class="collision" mesh="ring_collision_7" />
-      <geom class="collision" mesh="ring_collision_8" />
-      <geom class="collision" mesh="ring_collision_9" />
-      <geom class="collision" mesh="ring_collision_10" />
-      <geom class="collision" mesh="ring_collision_11" />
-      <geom class="collision" mesh="ring_collision_12" />
-      <geom class="collision" mesh="ring_collision_13" />
-      <geom class="collision" mesh="ring_collision_14" />
-      <geom class="collision" mesh="ring_collision_15" />
-      <geom class="collision" mesh="ring_collision_16" />
-      <geom class="collision" mesh="ring_collision_17" />
-      <geom class="collision" mesh="ring_collision_18" />
-      <geom class="collision" mesh="ring_collision_19" />
-      <geom class="collision" mesh="ring_collision_20" />
-      <geom class="collision" mesh="ring_collision_21" />
-      <geom class="collision" mesh="ring_collision_22" />
-      <geom class="collision" mesh="ring_collision_23" />
-      <geom class="collision" mesh="ring_collision_24" />
-      <geom class="collision" mesh="ring_collision_25" />
-      <geom class="collision" mesh="ring_collision_26" />
-      <geom class="collision" mesh="ring_collision_27" />
-      <geom class="collision" mesh="ring_collision_28" />
-      <geom class="collision" mesh="ring_collision_29" />
-      <geom class="collision" mesh="ring_collision_30" />
-      <geom class="collision" mesh="ring_collision_31" />
-      <geom class="collision" mesh="ring_collision_32" />
+      <freejoint/>
+      <inertial pos="0.0 0.0 0.0" quat="1 0 0 0" mass="1.5" diaginertia="0.00001 0.00001 0.00001"/>
+      <geom class="visual" mesh="satellite" material="satellite"/>
+      <geom class="collision" mesh="satellite_box" condim="6"/>
+      <geom class="collision" mesh="rafti_collision_1" condim="6"/>
+      <geom class="collision" mesh="rafti_collision_2" condim="6"/>
+      <geom class="collision" mesh="rafti_collision_3" condim="6"/>
+      <geom class="collision" mesh="rafti_collision_4" condim="6"/>
+      <geom class="collision" mesh="rafti_collision_5" condim="6"/>
+      <geom class="collision" mesh="ring_collision_1"/>
+      <geom class="collision" mesh="ring_collision_2"/>
+      <geom class="collision" mesh="ring_collision_3"/>
+      <geom class="collision" mesh="ring_collision_4"/>
+      <geom class="collision" mesh="ring_collision_5"/>
+      <geom class="collision" mesh="ring_collision_6"/>
+      <geom class="collision" mesh="ring_collision_7"/>
+      <geom class="collision" mesh="ring_collision_8"/>
+      <geom class="collision" mesh="ring_collision_9"/>
+      <geom class="collision" mesh="ring_collision_10"/>
+      <geom class="collision" mesh="ring_collision_11"/>
+      <geom class="collision" mesh="ring_collision_12"/>
+      <geom class="collision" mesh="ring_collision_13"/>
+      <geom class="collision" mesh="ring_collision_14"/>
+      <geom class="collision" mesh="ring_collision_15"/>
+      <geom class="collision" mesh="ring_collision_16"/>
+      <geom class="collision" mesh="ring_collision_17"/>
+      <geom class="collision" mesh="ring_collision_18"/>
+      <geom class="collision" mesh="ring_collision_19"/>
+      <geom class="collision" mesh="ring_collision_20"/>
+      <geom class="collision" mesh="ring_collision_21"/>
+      <geom class="collision" mesh="ring_collision_22"/>
+      <geom class="collision" mesh="ring_collision_23"/>
+      <geom class="collision" mesh="ring_collision_24"/>
+      <geom class="collision" mesh="ring_collision_25"/>
+      <geom class="collision" mesh="ring_collision_26"/>
+      <geom class="collision" mesh="ring_collision_27"/>
+      <geom class="collision" mesh="ring_collision_28"/>
+      <geom class="collision" mesh="ring_collision_29"/>
+      <geom class="collision" mesh="ring_collision_30"/>
+      <geom class="collision" mesh="ring_collision_31"/>
+      <geom class="collision" mesh="ring_collision_32"/>
     </body>
   </worldbody>
 </mujoco>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/scene.xml b/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/scene.xml
index 1134c64b..80bdd1c8 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/scene.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/description/mujoco/scene.xml
@@ -1,171 +1,120 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="kinova scene">
-  <include file="gen3_7dof.xml" />
-  <statistic center="0.3 0 0.4" extent="1" />
+  <include file="gen3_7dof.xml"/>
+  <statistic center="0.3 0 0.4" extent="1"/>
   <default>
-    <geom solref=".004 1" />
+    <geom solref=".004 1"/>
   </default>
 
   <visual>
-    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
-    <rgba haze="0.15 0.25 0.35 1" />
-    <global azimuth="120" elevation="-20" />
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
+    <rgba haze="0.15 0.25 0.35 1"/>
+    <global azimuth="120" elevation="-20"/>
   </visual>
 
   <asset>
-    <texture
-      type="skybox"
-      builtin="gradient"
-      rgb1="0.3 0.5 0.7"
-      rgb2="0 0 0"
-      width="512"
-      height="3072"
-    />
-    <texture
-      type="2d"
-      name="groundplane"
-      builtin="checker"
-      mark="edge"
-      rgb1="0.2 0.3 0.4"
-      rgb2="0.1 0.2 0.3"
-      markrgb="0.8 0.8 0.8"
-      width="300"
-      height="300"
-    />
-    <material
-      name="groundplane"
-      texture="groundplane"
-      texuniform="true"
-      texrepeat="5 5"
-      reflectance="0.2"
-    />
-    <texture
-      type="2d"
-      name="april_tag"
-      file="assets/black_white.png"
-      width="32"
-      height="32"
-    />
-    <material name="april_tag" texture="april_tag" />
-    <texture
-      type="2d"
-      name="satellite"
-      file="assets/satellite.png"
-      width="32"
-      height="32"
-    />
-    <material name="satellite" texture="satellite" />
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
+    <texture type="2d" name="april_tag" file="assets/black_white.png" width="32" height="32"/>
+    <material name="april_tag" texture="april_tag"/>
+    <texture type="2d" name="satellite" file="assets/satellite.png" width="32" height="32"/>
+    <material name="satellite" texture="satellite"/>
   </asset>
 
   <worldbody>
-    <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
-    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
-    <camera
-      name="scene_camera"
-      pos="0.0 0.3 0.55"
-      fovy="58"
-      mode="fixed"
-      resolution="640 480"
-      quat="0.365715 0.232986 -0.484157 -0.759975"
-    />
-    <site
-      name="scene_camera_optical_frame"
-      pos="0.0 0.3 0.55"
-      quat="0.365715 0.232986 -0.484157 -0.759975"
-      size="0.03"
-    />
+    <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
+    <camera name="scene_camera" pos="0.0 0.3 0.55" fovy="58" mode="fixed" resolution="640 480" quat="0.365715 0.232986 -0.484157 -0.759975"/>
+    <site name="scene_camera_optical_frame" pos="0.0 0.3 0.55" quat="0.365715 0.232986 -0.484157 -0.759975" size="0.03"/>
   </worldbody>
 
   <asset>
-    <mesh name="april_tag" file="april_tag.obj" />
-    <mesh name="satellite" file="satellite.obj" />
-    <mesh name="satellite_box" file="satellite_box.obj" />
-    <mesh name="rafti_collision_1" file="rafti_collision_1.obj" />
-    <mesh name="rafti_collision_2" file="rafti_collision_2.obj" />
-    <mesh name="rafti_collision_3" file="rafti_collision_3.obj" />
-    <mesh name="rafti_collision_4" file="rafti_collision_4.obj" />
-    <mesh name="rafti_collision_5" file="rafti_collision_5.obj" />
-    <mesh name="ring_collision_1" file="ring_collision_1.obj" />
-    <mesh name="ring_collision_2" file="ring_collision_2.obj" />
-    <mesh name="ring_collision_3" file="ring_collision_3.obj" />
-    <mesh name="ring_collision_4" file="ring_collision_4.obj" />
-    <mesh name="ring_collision_5" file="ring_collision_5.obj" />
-    <mesh name="ring_collision_6" file="ring_collision_6.obj" />
-    <mesh name="ring_collision_7" file="ring_collision_7.obj" />
-    <mesh name="ring_collision_8" file="ring_collision_8.obj" />
-    <mesh name="ring_collision_9" file="ring_collision_9.obj" />
-    <mesh name="ring_collision_10" file="ring_collision_10.obj" />
-    <mesh name="ring_collision_11" file="ring_collision_11.obj" />
-    <mesh name="ring_collision_12" file="ring_collision_12.obj" />
-    <mesh name="ring_collision_13" file="ring_collision_13.obj" />
-    <mesh name="ring_collision_14" file="ring_collision_14.obj" />
-    <mesh name="ring_collision_15" file="ring_collision_15.obj" />
-    <mesh name="ring_collision_16" file="ring_collision_16.obj" />
-    <mesh name="ring_collision_17" file="ring_collision_17.obj" />
-    <mesh name="ring_collision_18" file="ring_collision_18.obj" />
-    <mesh name="ring_collision_19" file="ring_collision_19.obj" />
-    <mesh name="ring_collision_20" file="ring_collision_20.obj" />
-    <mesh name="ring_collision_21" file="ring_collision_21.obj" />
-    <mesh name="ring_collision_22" file="ring_collision_22.obj" />
-    <mesh name="ring_collision_23" file="ring_collision_23.obj" />
-    <mesh name="ring_collision_24" file="ring_collision_24.obj" />
-    <mesh name="ring_collision_25" file="ring_collision_25.obj" />
-    <mesh name="ring_collision_26" file="ring_collision_26.obj" />
-    <mesh name="ring_collision_27" file="ring_collision_27.obj" />
-    <mesh name="ring_collision_28" file="ring_collision_28.obj" />
-    <mesh name="ring_collision_29" file="ring_collision_29.obj" />
-    <mesh name="ring_collision_30" file="ring_collision_30.obj" />
-    <mesh name="ring_collision_31" file="ring_collision_31.obj" />
-    <mesh name="ring_collision_32" file="ring_collision_32.obj" />
+    <mesh name="april_tag" file="april_tag.obj"/>
+    <mesh name="satellite" file="satellite.obj"/>
+    <mesh name="satellite_box" file="satellite_box.obj"/>
+    <mesh name="rafti_collision_1" file="rafti_collision_1.obj"/>
+    <mesh name="rafti_collision_2" file="rafti_collision_2.obj"/>
+    <mesh name="rafti_collision_3" file="rafti_collision_3.obj"/>
+    <mesh name="rafti_collision_4" file="rafti_collision_4.obj"/>
+    <mesh name="rafti_collision_5" file="rafti_collision_5.obj"/>
+    <mesh name="ring_collision_1" file="ring_collision_1.obj"/>
+    <mesh name="ring_collision_2" file="ring_collision_2.obj"/>
+    <mesh name="ring_collision_3" file="ring_collision_3.obj"/>
+    <mesh name="ring_collision_4" file="ring_collision_4.obj"/>
+    <mesh name="ring_collision_5" file="ring_collision_5.obj"/>
+    <mesh name="ring_collision_6" file="ring_collision_6.obj"/>
+    <mesh name="ring_collision_7" file="ring_collision_7.obj"/>
+    <mesh name="ring_collision_8" file="ring_collision_8.obj"/>
+    <mesh name="ring_collision_9" file="ring_collision_9.obj"/>
+    <mesh name="ring_collision_10" file="ring_collision_10.obj"/>
+    <mesh name="ring_collision_11" file="ring_collision_11.obj"/>
+    <mesh name="ring_collision_12" file="ring_collision_12.obj"/>
+    <mesh name="ring_collision_13" file="ring_collision_13.obj"/>
+    <mesh name="ring_collision_14" file="ring_collision_14.obj"/>
+    <mesh name="ring_collision_15" file="ring_collision_15.obj"/>
+    <mesh name="ring_collision_16" file="ring_collision_16.obj"/>
+    <mesh name="ring_collision_17" file="ring_collision_17.obj"/>
+    <mesh name="ring_collision_18" file="ring_collision_18.obj"/>
+    <mesh name="ring_collision_19" file="ring_collision_19.obj"/>
+    <mesh name="ring_collision_20" file="ring_collision_20.obj"/>
+    <mesh name="ring_collision_21" file="ring_collision_21.obj"/>
+    <mesh name="ring_collision_22" file="ring_collision_22.obj"/>
+    <mesh name="ring_collision_23" file="ring_collision_23.obj"/>
+    <mesh name="ring_collision_24" file="ring_collision_24.obj"/>
+    <mesh name="ring_collision_25" file="ring_collision_25.obj"/>
+    <mesh name="ring_collision_26" file="ring_collision_26.obj"/>
+    <mesh name="ring_collision_27" file="ring_collision_27.obj"/>
+    <mesh name="ring_collision_28" file="ring_collision_28.obj"/>
+    <mesh name="ring_collision_29" file="ring_collision_29.obj"/>
+    <mesh name="ring_collision_30" file="ring_collision_30.obj"/>
+    <mesh name="ring_collision_31" file="ring_collision_31.obj"/>
+    <mesh name="ring_collision_32" file="ring_collision_32.obj"/>
   </asset>
 
   <worldbody>
     <body name="satellite" pos=".43 -0.2 .133" quat="0.584012 0.0 0.0 0.811745">
-      <freejoint />
-      <inertial
-        pos="0.0 0.0 0.0"
-        quat="1 0 0 0"
-        mass="1.5"
-        diaginertia="0.00001 0.00001 0.00001"
-      />
-      <geom class="visual" mesh="satellite" material="satellite" />
-      <geom class="collision" mesh="satellite_box" condim="6" />
-      <geom class="collision" mesh="rafti_collision_1" condim="6" />
-      <geom class="collision" mesh="rafti_collision_2" condim="6" />
-      <geom class="collision" mesh="rafti_collision_3" condim="6" />
-      <geom class="collision" mesh="rafti_collision_4" condim="6" />
-      <geom class="collision" mesh="rafti_collision_5" condim="6" />
-      <geom class="collision" mesh="ring_collision_1" />
-      <geom class="collision" mesh="ring_collision_2" />
-      <geom class="collision" mesh="ring_collision_3" />
-      <geom class="collision" mesh="ring_collision_4" />
-      <geom class="collision" mesh="ring_collision_5" />
-      <geom class="collision" mesh="ring_collision_6" />
-      <geom class="collision" mesh="ring_collision_7" />
-      <geom class="collision" mesh="ring_collision_8" />
-      <geom class="collision" mesh="ring_collision_9" />
-      <geom class="collision" mesh="ring_collision_10" />
-      <geom class="collision" mesh="ring_collision_11" />
-      <geom class="collision" mesh="ring_collision_12" />
-      <geom class="collision" mesh="ring_collision_13" />
-      <geom class="collision" mesh="ring_collision_14" />
-      <geom class="collision" mesh="ring_collision_15" />
-      <geom class="collision" mesh="ring_collision_16" />
-      <geom class="collision" mesh="ring_collision_17" />
-      <geom class="collision" mesh="ring_collision_18" />
-      <geom class="collision" mesh="ring_collision_19" />
-      <geom class="collision" mesh="ring_collision_20" />
-      <geom class="collision" mesh="ring_collision_21" />
-      <geom class="collision" mesh="ring_collision_22" />
-      <geom class="collision" mesh="ring_collision_23" />
-      <geom class="collision" mesh="ring_collision_24" />
-      <geom class="collision" mesh="ring_collision_25" />
-      <geom class="collision" mesh="ring_collision_26" />
-      <geom class="collision" mesh="ring_collision_27" />
-      <geom class="collision" mesh="ring_collision_28" />
-      <geom class="collision" mesh="ring_collision_29" />
-      <geom class="collision" mesh="ring_collision_30" />
-      <geom class="collision" mesh="ring_collision_31" />
-      <geom class="collision" mesh="ring_collision_32" />
+      <freejoint/>
+      <inertial pos="0.0 0.0 0.0" quat="1 0 0 0" mass="1.5" diaginertia="0.00001 0.00001 0.00001"/>
+      <geom class="visual" mesh="satellite" material="satellite"/>
+      <geom class="collision" mesh="satellite_box" condim="6"/>
+      <geom class="collision" mesh="rafti_collision_1" condim="6"/>
+      <geom class="collision" mesh="rafti_collision_2" condim="6"/>
+      <geom class="collision" mesh="rafti_collision_3" condim="6"/>
+      <geom class="collision" mesh="rafti_collision_4" condim="6"/>
+      <geom class="collision" mesh="rafti_collision_5" condim="6"/>
+      <geom class="collision" mesh="ring_collision_1"/>
+      <geom class="collision" mesh="ring_collision_2"/>
+      <geom class="collision" mesh="ring_collision_3"/>
+      <geom class="collision" mesh="ring_collision_4"/>
+      <geom class="collision" mesh="ring_collision_5"/>
+      <geom class="collision" mesh="ring_collision_6"/>
+      <geom class="collision" mesh="ring_collision_7"/>
+      <geom class="collision" mesh="ring_collision_8"/>
+      <geom class="collision" mesh="ring_collision_9"/>
+      <geom class="collision" mesh="ring_collision_10"/>
+      <geom class="collision" mesh="ring_collision_11"/>
+      <geom class="collision" mesh="ring_collision_12"/>
+      <geom class="collision" mesh="ring_collision_13"/>
+      <geom class="collision" mesh="ring_collision_14"/>
+      <geom class="collision" mesh="ring_collision_15"/>
+      <geom class="collision" mesh="ring_collision_16"/>
+      <geom class="collision" mesh="ring_collision_17"/>
+      <geom class="collision" mesh="ring_collision_18"/>
+      <geom class="collision" mesh="ring_collision_19"/>
+      <geom class="collision" mesh="ring_collision_20"/>
+      <geom class="collision" mesh="ring_collision_21"/>
+      <geom class="collision" mesh="ring_collision_22"/>
+      <geom class="collision" mesh="ring_collision_23"/>
+      <geom class="collision" mesh="ring_collision_24"/>
+      <geom class="collision" mesh="ring_collision_25"/>
+      <geom class="collision" mesh="ring_collision_26"/>
+      <geom class="collision" mesh="ring_collision_27"/>
+      <geom class="collision" mesh="ring_collision_28"/>
+      <geom class="collision" mesh="ring_collision_29"/>
+      <geom class="collision" mesh="ring_collision_30"/>
+      <geom class="collision" mesh="ring_collision_31"/>
+      <geom class="collision" mesh="ring_collision_32"/>
     </body>
   </worldbody>
 </mujoco>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/launch/agent_bridge.launch.xml b/src/moveit_pro_kinova_configs/kinova_sim/launch/agent_bridge.launch.xml
index 1179679a..8eeec7d9 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/launch/agent_bridge.launch.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/launch/agent_bridge.launch.xml
@@ -1,5 +1,4 @@
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
 </launch>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/april_tag_object_registration.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/april_tag_object_registration.xml
index 453c9e44..2d819bc8 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/april_tag_object_registration.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/april_tag_object_registration.xml
@@ -1,264 +1,89 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="April Tag Object Registration">
   <!--//////////-->
-  <BehaviorTree
-    ID="April Tag Object Registration"
-    _description="Estimate the pose of an object using ICP."
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="April Tag Object Registration" _description="Estimate the pose of an object using ICP." _favorite="true" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="View Satellite"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
-      <Action ID="ClearSnapshot" />
-      <SubTree ID="Open Gripper" _collapsed="true" />
-      <SubTree ID="Take Snapshot" _collapsed="true" />
-      <SubTree
-        ID="Sample April Tag"
-        _collapsed="true"
-        num_samples="1"
-        tag_id="5"
-        apriltag_config="51mm_apriltag_detection_config.yaml"
-        max_distance="0.02"
-        max_rotation="0.2"
-        avg_pose="{tag_pose}"
-      />
-      <Action
-        ID="TransformPoseFrame"
-        input_pose="{tag_pose}"
-        target_frame_id="world"
-        output_pose="{tag_pose_world}"
-      />
+      <SubTree ID="Move to Waypoint" waypoint_name="View Satellite" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Open Gripper" _collapsed="true"/>
+      <SubTree ID="Take Snapshot" _collapsed="true"/>
+      <SubTree ID="Sample April Tag" _collapsed="true" num_samples="1" tag_id="5" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+      <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
       <Control ID="Fallback">
         <Decorator ID="Timeout" msec="1000">
-          <Action
-            ID="PublishStaticFrame"
-            pose="{tag_pose_world}"
-            child_frame_id="tag_frame"
-            publish_rate="5"
-          />
+          <Action ID="PublishStaticFrame" pose="{tag_pose_world}" child_frame_id="tag_frame" publish_rate="5"/>
         </Decorator>
         <Decorator ID="Timeout" msec="1000">
           <Control ID="Sequence">
-            <Action
-              ID="TransformPose"
-              input_pose="{tag_pose_world}"
-              translation_xyz="0.104533 ;-0.104463;-0.132927"
-              quaternion_xyzw="0.0;0.0;1.0;0.0"
-              output_pose="{object_stamped_pose_estimate}"
-            />
-            <Action
-              ID="PublishStaticFrame"
-              pose="{object_stamped_pose_estimate}"
-              child_frame_id="object_frame"
-              publish_rate="5"
-            />
+            <Action ID="TransformPose" input_pose="{tag_pose_world}" translation_xyz="0.104533 ;-0.104463;-0.132927" quaternion_xyzw="0.0;0.0;1.0;0.0" output_pose="{object_stamped_pose_estimate}"/>
+            <Action ID="PublishStaticFrame" pose="{object_stamped_pose_estimate}" child_frame_id="object_frame" publish_rate="5"/>
           </Control>
         </Decorator>
         <Control ID="Sequence">
-          <Action
-            ID="LoadPointCloudFromFile"
-            file_path="descriptions/miscellaneous/mock_satellite.stl"
-            package_name="picknik_accessories"
-            frame_id="object_frame"
-            num_sampled_points="3000"
-            random_seed="1234"
-            point_cloud="{model_point_cloud}"
-            scale="1.0"
-            color="255;150;0"
-          />
+          <Action ID="LoadPointCloudFromFile" file_path="descriptions/miscellaneous/mock_satellite.stl" package_name="picknik_accessories" frame_id="object_frame" num_sampled_points="3000" random_seed="1234" point_cloud="{model_point_cloud}" scale="1.0" color="255;150;0"/>
           <Control ID="Fallback">
             <Decorator ID="Timeout" msec="1000">
-              <Action
-                ID="PublishStaticFrame"
-                pose="{object_stamped_pose_estimate}"
-                child_frame_id="object_frame"
-                publish_rate="5"
-              />
+              <Action ID="PublishStaticFrame" pose="{object_stamped_pose_estimate}" child_frame_id="object_frame" publish_rate="5"/>
             </Decorator>
-            <Action ID="AlwaysSuccess" />
+            <Action ID="AlwaysSuccess"/>
           </Control>
-          <Action
-            ID="TransformPointCloudFrame"
-            input_cloud="{model_point_cloud}"
-            target_frame="world"
-            output_cloud="{model_point_cloud}"
-          />
-          <Action
-            ID="GetSyncedImageAndPointCloud"
-            point_cloud_topic_name="/wrist_camera/points"
-            rgb_image_topic_name="/wrist_camera/color"
-            rgb_camera_info_topic_name="/wrist_camera/camera_info"
-            point_cloud="{point_cloud}"
-            rgb_image="{rgb_image}"
-            rgb_camera_info="{rgb_camera_info}"
-          />
-          <Action
-            ID="TransformPointCloudFrame"
-            input_cloud="{point_cloud}"
-            target_frame="world"
-            output_cloud="{point_cloud}"
-          />
-          <Action
-            ID="RegisterPointClouds"
-            base_point_cloud="{model_point_cloud}"
-            target_point_cloud="{point_cloud}"
-            max_correspondence_distance="0.01"
-            target_pose_in_base_frame="{model_to_real_pose}"
-            max_iterations="5"
-          />
-          <Action
-            ID="TransformPointCloud"
-            input_cloud="{model_point_cloud}"
-            transform_pose="{model_to_real_pose}"
-            output_cloud="{aligned_model_cloud}"
-          />
+          <Action ID="TransformPointCloudFrame" input_cloud="{model_point_cloud}" target_frame="world" output_cloud="{model_point_cloud}"/>
+          <Action ID="GetSyncedImageAndPointCloud" point_cloud_topic_name="/wrist_camera/points" rgb_image_topic_name="/wrist_camera/color" rgb_camera_info_topic_name="/wrist_camera/camera_info" point_cloud="{point_cloud}" rgb_image="{rgb_image}" rgb_camera_info="{rgb_camera_info}"/>
+          <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" target_frame="world" output_cloud="{point_cloud}"/>
+          <Action ID="RegisterPointClouds" base_point_cloud="{model_point_cloud}" target_point_cloud="{point_cloud}" max_correspondence_distance="0.01" target_pose_in_base_frame="{model_to_real_pose}" max_iterations="5"/>
+          <Action ID="TransformPointCloud" input_cloud="{model_point_cloud}" transform_pose="{model_to_real_pose}" output_cloud="{aligned_model_cloud}"/>
           <Control ID="Fallback">
             <Decorator ID="Timeout" msec="100">
-              <Action
-                ID="PublishStaticFrame"
-                pose="{object_stamped_pose_estimate}"
-                child_frame_id="object_frame"
-                publish_rate="5"
-              />
+              <Action ID="PublishStaticFrame" pose="{object_stamped_pose_estimate}" child_frame_id="object_frame" publish_rate="5"/>
             </Decorator>
-            <Action ID="AlwaysSuccess" />
+            <Action ID="AlwaysSuccess"/>
           </Control>
-          <Action
-            ID="SendPointCloudToUI"
-            point_cloud="{aligned_model_cloud}"
-            pcd_topic="/pcd_pointcloud_captures"
-          />
-          <Action
-            ID="TransformPoseWithPose"
-            input_pose="{object_stamped_pose_estimate}"
-            transform_pose="{model_to_real_pose}"
-            output_pose="{model_world}"
-          />
+          <Action ID="SendPointCloudToUI" point_cloud="{aligned_model_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
+          <Action ID="TransformPoseWithPose" input_pose="{object_stamped_pose_estimate}" transform_pose="{model_to_real_pose}" output_pose="{model_world}"/>
           <Control ID="Fallback">
             <Decorator ID="Timeout" msec="1000">
-              <Action
-                ID="PublishStaticFrame"
-                pose="{model_world}"
-                child_frame_id="object_frame_new"
-                publish_rate="5"
-              />
+              <Action ID="PublishStaticFrame" pose="{model_world}" child_frame_id="object_frame_new" publish_rate="5"/>
             </Decorator>
-            <Action ID="AlwaysSuccess" />
+            <Action ID="AlwaysSuccess"/>
           </Control>
-          <Action
-            ID="TransformPoseFrame"
-            input_pose="{model_world}"
-            target_frame_id="object_frame_new"
-            output_pose="{model_object}"
-          />
-          <Action
-            ID="TransformPose"
-            input_pose="{model_object}"
-            translation_xyz="0.155;-0.1075;0.0"
-            quaternion_xyzw="0.5;-0.5;-0.5;0.5"
-            output_pose="{hole_object}"
-          />
-          <Action
-            ID="TransformPoseFrame"
-            input_pose="{hole_object}"
-            target_frame_id="world"
-            output_pose="{hole_object}"
-          />
+          <Action ID="TransformPoseFrame" input_pose="{model_world}" target_frame_id="object_frame_new" output_pose="{model_object}"/>
+          <Action ID="TransformPose" input_pose="{model_object}" translation_xyz="0.155;-0.1075;0.0" quaternion_xyzw="0.5;-0.5;-0.5;0.5" output_pose="{hole_object}"/>
+          <Action ID="TransformPoseFrame" input_pose="{hole_object}" target_frame_id="world" output_pose="{hole_object}"/>
           <Control ID="Fallback">
             <Decorator ID="Timeout" msec="1000">
-              <Action
-                ID="PublishStaticFrame"
-                pose="{hole_object}"
-                child_frame_id="object_hole"
-                publish_rate="5"
-              />
+              <Action ID="PublishStaticFrame" pose="{hole_object}" child_frame_id="object_hole" publish_rate="5"/>
             </Decorator>
-            <Action ID="AlwaysSuccess" />
+            <Action ID="AlwaysSuccess"/>
           </Control>
         </Control>
       </Control>
-      <Action
-        ID="TransformPose"
-        input_pose="{hole_object}"
-        translation_xyz="0.0;0.0;-0.11"
-        quaternion_xyzw="0.0;0.0;0.0;1.0"
-        output_pose="{goal_pose}"
-      />
-      <Action
-        ID="TransformPose"
-        input_pose="{goal_pose}"
-        translation_xyz="0.0;0.1;0.0"
-        quaternion_xyzw="0.0;0.0;0.0;1.0"
-        output_pose="{initial_goal_pose}"
-      />
+      <Action ID="TransformPose" input_pose="{hole_object}" translation_xyz="0.0;0.0;-0.11" quaternion_xyzw="0.0;0.0;0.0;1.0" output_pose="{goal_pose}"/>
+      <Action ID="TransformPose" input_pose="{goal_pose}" translation_xyz="0.0;0.1;0.0" quaternion_xyzw="0.0;0.0;0.0;1.0" output_pose="{initial_goal_pose}"/>
       <Control ID="Fallback">
         <Decorator ID="Timeout" msec="1000">
-          <Action
-            ID="PublishStaticFrame"
-            pose="{goal_pose}"
-            child_frame_id="goal_pose"
-            publish_rate="5"
-          />
+          <Action ID="PublishStaticFrame" pose="{goal_pose}" child_frame_id="goal_pose" publish_rate="5"/>
         </Decorator>
-        <Action ID="AlwaysSuccess" />
+        <Action ID="AlwaysSuccess"/>
       </Control>
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{pose_stamped_vector}"
-        input="{initial_goal_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{pose_stamped_vector}"
-        input="{goal_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{pose_stamped_vector}"
-        input="{hole_object}"
-      />
+      <Action ID="AddPoseStampedToVector" vector="{pose_stamped_vector}" input="{initial_goal_pose}"/>
+      <Action ID="AddPoseStampedToVector" vector="{pose_stamped_vector}" input="{goal_pose}"/>
+      <Action ID="AddPoseStampedToVector" vector="{pose_stamped_vector}" input="{hole_object}"/>
       <Control ID="Fallback">
-        <Action
-          ID="PlanCartesianPath"
-          trajectory_sampling_rate="100.000000"
-          acceleration_scale_factor="0.5"
-          debug_solution="{debug_solution}"
-          velocity_scale_factor="0.2"
-          blending_radius="0.020000"
-          position_only="false"
-          joint_trajectory_msg="{joint_trajectory_msg}"
-          tip_link="grasp_link"
-          planning_group_name="manipulator"
-          path="{pose_stamped_vector}"
-        />
-        <Action
-          ID="WaitForUserTrajectoryApproval"
-          solution="{debug_solution}"
-        />
+        <Action ID="PlanCartesianPath" trajectory_sampling_rate="100.000000" acceleration_scale_factor="0.5" debug_solution="{debug_solution}" velocity_scale_factor="0.2" blending_radius="0.020000" position_only="false" joint_trajectory_msg="{joint_trajectory_msg}" tip_link="grasp_link" planning_group_name="manipulator" path="{pose_stamped_vector}"/>
+        <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
       </Control>
-      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
       <Control ID="Fallback">
-        <Action
-          ID="ExecuteFollowJointTrajectory"
-          joint_trajectory_msg="{joint_trajectory_msg}"
-          execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-          goal_time_tolerance="1.0"
-          goal_position_tolerance="0.01"
-        />
-        <Action ID="AlwaysSuccess" />
+        <Action ID="ExecuteFollowJointTrajectory" joint_trajectory_msg="{joint_trajectory_msg}" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_time_tolerance="1.0" goal_position_tolerance="0.01"/>
+        <Action ID="AlwaysSuccess"/>
       </Control>
-      <SubTree ID="Close Gripper" _collapsed="true" />
+      <SubTree ID="Close Gripper" _collapsed="true"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="April Tag Object Registration">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/close_gripper.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/close_gripper.xml
index 6997559e..95676719 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/close_gripper.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/close_gripper.xml
@@ -1,19 +1,15 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Close Gripper">
   <BehaviorTree ID="Close Gripper" _description="Close the gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.8"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.8"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Close Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Grasping"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/compliant_grasp_rafti.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/compliant_grasp_rafti.xml
index 79acea5c..a598070c 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/compliant_grasp_rafti.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/compliant_grasp_rafti.xml
@@ -1,252 +1,86 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Compliant Grasp RAFTI">
   <!--//////////-->
-  <BehaviorTree
-    ID="Compliant Grasp RAFTI"
-    _description="Grasp RAFTI with admittance."
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Compliant Grasp RAFTI" _description="Grasp RAFTI with admittance." _favorite="true" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="ClearSnapshot" />
-      <SubTree ID="Open Gripper" _collapsed="true" />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="View Satellite"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller"
-      />
-      <SubTree ID="Take Snapshot" _collapsed="false" />
-      <SubTree
-        ID="Sample April Tag"
-        _collapsed="false"
-        num_samples="1"
-        tag_id="5"
-        apriltag_config="51mm_apriltag_detection_config.yaml"
-        max_distance="0.02"
-        max_rotation="0.2"
-        avg_pose="{tag_pose}"
-      />
-      <Action
-        ID="TransformPoseFrame"
-        input_pose="{tag_pose}"
-        target_frame_id="world"
-        output_pose="{tag_pose_world}"
-      />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Open Gripper" _collapsed="true"/>
+      <SubTree ID="Move to Waypoint" waypoint_name="View Satellite" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
+      <SubTree ID="Take Snapshot" _collapsed="false"/>
+      <SubTree ID="Sample April Tag" _collapsed="false" num_samples="1" tag_id="5" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+      <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
       <Control ID="Fallback">
         <Decorator ID="Timeout" msec="1000">
-          <Action
-            ID="PublishStaticFrame"
-            pose="{tag_pose_world}"
-            child_frame_id="tag_frame"
-            publish_rate="5"
-          />
+          <Action ID="PublishStaticFrame" pose="{tag_pose_world}" child_frame_id="tag_frame" publish_rate="5"/>
         </Decorator>
         <Decorator ID="Timeout" msec="1000">
           <Control ID="Sequence">
-            <Action
-              ID="TransformPose"
-              input_pose="{tag_pose_world}"
-              translation_xyz="0.104533 ;-0.104463;-0.132927"
-              quaternion_xyzw="0.0;0.0;1.0;0.0"
-              output_pose="{object_stamped_pose_estimate}"
-            />
-            <Action
-              ID="PublishStaticFrame"
-              pose="{object_stamped_pose_estimate}"
-              child_frame_id="object_frame"
-              publish_rate="5"
-            />
+            <Action ID="TransformPose" input_pose="{tag_pose_world}" translation_xyz="0.104533 ;-0.104463;-0.132927" quaternion_xyzw="0.0;0.0;1.0;0.0" output_pose="{object_stamped_pose_estimate}"/>
+            <Action ID="PublishStaticFrame" pose="{object_stamped_pose_estimate}" child_frame_id="object_frame" publish_rate="5"/>
           </Control>
         </Decorator>
         <Control ID="Sequence">
-          <Action
-            ID="LoadPointCloudFromFile"
-            file_path="descriptions/miscellaneous/mock_satellite.stl"
-            package_name="picknik_accessories"
-            frame_id="object_frame"
-            num_sampled_points="3000"
-            random_seed="1234"
-            point_cloud="{model_point_cloud}"
-            scale="1.0"
-            color="255;150;0"
-          />
+          <Action ID="LoadPointCloudFromFile" file_path="descriptions/miscellaneous/mock_satellite.stl" package_name="picknik_accessories" frame_id="object_frame" num_sampled_points="3000" random_seed="1234" point_cloud="{model_point_cloud}" scale="1.0" color="255;150;0"/>
           <Control ID="Fallback">
             <Decorator ID="Timeout" msec="1000">
-              <Action
-                ID="PublishStaticFrame"
-                pose="{object_stamped_pose_estimate}"
-                child_frame_id="object_frame"
-                publish_rate="5"
-              />
+              <Action ID="PublishStaticFrame" pose="{object_stamped_pose_estimate}" child_frame_id="object_frame" publish_rate="5"/>
             </Decorator>
-            <Action ID="AlwaysSuccess" />
+            <Action ID="AlwaysSuccess"/>
           </Control>
-          <Action
-            ID="TransformPointCloudFrame"
-            input_cloud="{model_point_cloud}"
-            target_frame="world"
-            output_cloud="{model_point_cloud}"
-          />
-          <Action
-            ID="GetSyncedImageAndPointCloud"
-            point_cloud="{point_cloud}"
-            rgb_image="{rgb_image}"
-            point_cloud_topic_name="/wrist_camera/points"
-            rgb_camera_info_topic_name="/wrist_camera/camera_info"
-            rgb_image_topic_name="/wrist_camera/color"
-            rgb_camera_info="{rgb_camera_info}"
-          />
-          <Action
-            ID="TransformPointCloudFrame"
-            input_cloud="{point_cloud}"
-            target_frame="world"
-            output_cloud="{point_cloud}"
-          />
-          <Action
-            ID="RegisterPointClouds"
-            base_point_cloud="{model_point_cloud}"
-            target_point_cloud="{point_cloud}"
-            max_iterations="30"
-            max_correspondence_distance="0.02"
-            target_pose_in_base_frame="{model_to_real_pose}"
-          />
-          <Action
-            ID="TransformPointCloud"
-            input_cloud="{model_point_cloud}"
-            transform_pose="{model_to_real_pose}"
-            output_cloud="{aligned_model_cloud}"
-          />
+          <Action ID="TransformPointCloudFrame" input_cloud="{model_point_cloud}" target_frame="world" output_cloud="{model_point_cloud}"/>
+          <Action ID="GetSyncedImageAndPointCloud" point_cloud="{point_cloud}" rgb_image="{rgb_image}" point_cloud_topic_name="/wrist_camera/points" rgb_camera_info_topic_name="/wrist_camera/camera_info" rgb_image_topic_name="/wrist_camera/color" rgb_camera_info="{rgb_camera_info}"/>
+          <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" target_frame="world" output_cloud="{point_cloud}"/>
+          <Action ID="RegisterPointClouds" base_point_cloud="{model_point_cloud}" target_point_cloud="{point_cloud}" max_iterations="30" max_correspondence_distance="0.02" target_pose_in_base_frame="{model_to_real_pose}"/>
+          <Action ID="TransformPointCloud" input_cloud="{model_point_cloud}" transform_pose="{model_to_real_pose}" output_cloud="{aligned_model_cloud}"/>
           <Control ID="Fallback">
             <Decorator ID="Timeout" msec="100">
-              <Action
-                ID="PublishStaticFrame"
-                pose="{object_stamped_pose_estimate}"
-                child_frame_id="object_frame"
-                publish_rate="5"
-              />
+              <Action ID="PublishStaticFrame" pose="{object_stamped_pose_estimate}" child_frame_id="object_frame" publish_rate="5"/>
             </Decorator>
-            <Action ID="AlwaysSuccess" />
+            <Action ID="AlwaysSuccess"/>
           </Control>
-          <Action
-            ID="SendPointCloudToUI"
-            point_cloud="{aligned_model_cloud}"
-            pcd_topic="/pcd_pointcloud_captures"
-          />
-          <Action
-            ID="TransformPoseWithPose"
-            input_pose="{object_stamped_pose_estimate}"
-            transform_pose="{model_to_real_pose}"
-            output_pose="{model_world}"
-          />
+          <Action ID="SendPointCloudToUI" point_cloud="{aligned_model_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
+          <Action ID="TransformPoseWithPose" input_pose="{object_stamped_pose_estimate}" transform_pose="{model_to_real_pose}" output_pose="{model_world}"/>
           <Control ID="Fallback">
             <Decorator ID="Timeout" msec="1000">
-              <Action
-                ID="PublishStaticFrame"
-                pose="{model_world}"
-                child_frame_id="object_frame_new"
-                publish_rate="5"
-              />
+              <Action ID="PublishStaticFrame" pose="{model_world}" child_frame_id="object_frame_new" publish_rate="5"/>
             </Decorator>
-            <Action ID="AlwaysSuccess" />
+            <Action ID="AlwaysSuccess"/>
           </Control>
-          <Action
-            ID="TransformPoseFrame"
-            input_pose="{model_world}"
-            target_frame_id="object_frame_new"
-            output_pose="{model_object}"
-          />
-          <Action
-            ID="TransformPose"
-            input_pose="{model_object}"
-            translation_xyz="0.0;0.025;0.3"
-            quaternion_xyzw="-0.707107;0.707107;0.0;0.0"
-            output_pose="{hole_object}"
-          />
-          <Action
-            ID="TransformPoseFrame"
-            input_pose="{hole_object}"
-            target_frame_id="world"
-            output_pose="{hole_object}"
-          />
+          <Action ID="TransformPoseFrame" input_pose="{model_world}" target_frame_id="object_frame_new" output_pose="{model_object}"/>
+          <Action ID="TransformPose" input_pose="{model_object}" translation_xyz="0.0;0.025;0.3" quaternion_xyzw="-0.707107;0.707107;0.0;0.0" output_pose="{hole_object}"/>
+          <Action ID="TransformPoseFrame" input_pose="{hole_object}" target_frame_id="world" output_pose="{hole_object}"/>
           <Control ID="Fallback">
             <Decorator ID="Timeout" msec="1000">
-              <Action
-                ID="PublishStaticFrame"
-                pose="{hole_object}"
-                child_frame_id="object_hole"
-                publish_rate="5"
-              />
+              <Action ID="PublishStaticFrame" pose="{hole_object}" child_frame_id="object_hole" publish_rate="5"/>
             </Decorator>
-            <Action ID="AlwaysSuccess" />
+            <Action ID="AlwaysSuccess"/>
           </Control>
         </Control>
       </Control>
-      <Action
-        ID="InitializeMTCTask"
-        task_id=""
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        task="{mtc_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
-      <Action ID="SetupMTCPlanToPose" target_pose="{hole_object}" />
-      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
-      <Action ID="ExecuteMTCTask" solution="{mtc_solution}" />
-      <Action
-        ID="TransformPose"
-        input_pose="{hole_object}"
-        translation_xyz="0.0;0.0;-0.1"
-        quaternion_xyzw="0.0;1.0;0.0;0.0"
-        output_pose="{goal_pose}"
-      />
-      <Action
-        ID="InitializeMTCTask"
-        task_id=""
-        controller_names="/joint_trajectory_controller_chained_open_door /robotiq_gripper_controller"
-        task="{mtc_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
-      <Action
-        ID="SetupMTCUpdateGroupCollisionRule"
-        group_name="gripper"
-        object_name="&lt;octomap&gt;"
-        allow_collision="true"
-        task="{mtc_task}"
-      />
-      <Action
-        ID="SetupMTCMoveAlongFrameAxis"
-        task="{mtc_task}"
-        hand_frame="grasp_link"
-        axis_frame="grasp_link"
-        axis_x="0.0"
-        axis_y="0.0"
-        axis_z="1.0"
-        max_distance="0.1"
-        min_distance="0"
-        planning_group_name="manipulator"
-        velocity_scale=".2"
-        acceleration_scale="1.0"
-      />
-      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
-      <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true" />
-      <Action
-        ID="ActivateControllers"
-        controller_names="admittance_controller_open_door"
-      />
-      <Action ID="ExecuteMTCTask" solution="{mtc_solution}" />
-      <SubTree ID="Close Gripper" _collapsed="true" />
+      <Action ID="InitializeMTCTask" task_id="" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{mtc_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
+      <Action ID="SetupMTCPlanToPose" target_pose="{hole_object}"/>
+      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{mtc_solution}"/>
+      <Action ID="TransformPose" input_pose="{hole_object}" translation_xyz="0.0;0.0;-0.1" quaternion_xyzw="0.0;1.0;0.0;0.0" output_pose="{goal_pose}"/>
+      <Action ID="InitializeMTCTask" task_id="" controller_names="/joint_trajectory_controller_chained_open_door /robotiq_gripper_controller" task="{mtc_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
+      <Action ID="SetupMTCUpdateGroupCollisionRule" group_name="gripper" object_name="&lt;octomap&gt;" allow_collision="true" task="{mtc_task}"/>
+      <Action ID="SetupMTCMoveAlongFrameAxis" task="{mtc_task}" hand_frame="grasp_link" axis_frame="grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.1" min_distance="0" planning_group_name="manipulator" velocity_scale=".2" acceleration_scale="1.0"/>
+      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}"/>
+      <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
+      <Action ID="ActivateControllers" controller_names="admittance_controller_open_door"/>
+      <Action ID="ExecuteMTCTask" solution="{mtc_solution}"/>
+      <SubTree ID="Close Gripper" _collapsed="true"/>
       <Decorator ID="Delay" delay_msec="1000">
-        <Action
-          ID="ActivateControllers"
-          controller_names="joint_trajectory_controller"
-        />
+        <Action ID="ActivateControllers" controller_names="joint_trajectory_controller"/>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Compliant Grasp RAFTI">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/compliant_grasp_rafti_vfc.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/compliant_grasp_rafti_vfc.xml
index cdcc09c2..8f88a3ff 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/compliant_grasp_rafti_vfc.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/compliant_grasp_rafti_vfc.xml
@@ -1,267 +1,91 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Compliant Grasp RAFTI VFC">
   <!--//////////-->
-  <BehaviorTree
-    ID="Compliant Grasp RAFTI VFC"
-    _description="Grasp RAFTI with admittance."
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Compliant Grasp RAFTI VFC" _description="Grasp RAFTI with admittance." _favorite="true" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="ClearSnapshot" />
-      <SubTree ID="Open Gripper" _collapsed="true" />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="View Satellite"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller"
-      />
-      <SubTree ID="Take Snapshot" _collapsed="false" />
-      <SubTree
-        ID="Sample April Tag"
-        _collapsed="false"
-        num_samples="1"
-        tag_id="5"
-        apriltag_config="51mm_apriltag_detection_config.yaml"
-        max_distance="0.02"
-        max_rotation="0.2"
-        avg_pose="{tag_pose}"
-      />
-      <Action
-        ID="TransformPoseFrame"
-        input_pose="{tag_pose}"
-        target_frame_id="world"
-        output_pose="{tag_pose_world}"
-      />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Open Gripper" _collapsed="true"/>
+      <SubTree ID="Move to Waypoint" waypoint_name="View Satellite" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
+      <SubTree ID="Take Snapshot" _collapsed="false"/>
+      <SubTree ID="Sample April Tag" _collapsed="false" num_samples="1" tag_id="5" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+      <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
       <Control ID="Fallback">
         <Decorator ID="Timeout" msec="1000">
-          <Action
-            ID="PublishStaticFrame"
-            pose="{tag_pose_world}"
-            child_frame_id="tag_frame"
-            publish_rate="5"
-          />
+          <Action ID="PublishStaticFrame" pose="{tag_pose_world}" child_frame_id="tag_frame" publish_rate="5"/>
         </Decorator>
         <Decorator ID="Timeout" msec="1000">
           <Control ID="Sequence">
-            <Action
-              ID="TransformPose"
-              input_pose="{tag_pose_world}"
-              translation_xyz="0.104533 ;-0.104463;-0.132927"
-              quaternion_xyzw="0.0;0.0;1.0;0.0"
-              output_pose="{object_stamped_pose_estimate}"
-            />
-            <Action
-              ID="PublishStaticFrame"
-              pose="{object_stamped_pose_estimate}"
-              child_frame_id="object_frame"
-              publish_rate="5"
-            />
+            <Action ID="TransformPose" input_pose="{tag_pose_world}" translation_xyz="0.104533 ;-0.104463;-0.132927" quaternion_xyzw="0.0;0.0;1.0;0.0" output_pose="{object_stamped_pose_estimate}"/>
+            <Action ID="PublishStaticFrame" pose="{object_stamped_pose_estimate}" child_frame_id="object_frame" publish_rate="5"/>
           </Control>
         </Decorator>
         <Control ID="Sequence">
-          <Action
-            ID="LoadPointCloudFromFile"
-            file_path="descriptions/miscellaneous/mock_satellite.stl"
-            package_name="picknik_accessories"
-            frame_id="object_frame"
-            num_sampled_points="3000"
-            random_seed="1234"
-            point_cloud="{model_point_cloud}"
-            scale="1.0"
-            color="255;150;0"
-          />
+          <Action ID="LoadPointCloudFromFile" file_path="descriptions/miscellaneous/mock_satellite.stl" package_name="picknik_accessories" frame_id="object_frame" num_sampled_points="3000" random_seed="1234" point_cloud="{model_point_cloud}" scale="1.0" color="255;150;0"/>
           <Control ID="Fallback">
             <Decorator ID="Timeout" msec="1000">
-              <Action
-                ID="PublishStaticFrame"
-                pose="{object_stamped_pose_estimate}"
-                child_frame_id="object_frame"
-                publish_rate="5"
-              />
+              <Action ID="PublishStaticFrame" pose="{object_stamped_pose_estimate}" child_frame_id="object_frame" publish_rate="5"/>
             </Decorator>
-            <Action ID="AlwaysSuccess" />
+            <Action ID="AlwaysSuccess"/>
           </Control>
-          <Action
-            ID="TransformPointCloudFrame"
-            input_cloud="{model_point_cloud}"
-            target_frame="world"
-            output_cloud="{model_point_cloud}"
-          />
-          <Action
-            ID="GetSyncedImageAndPointCloud"
-            point_cloud="{point_cloud}"
-            rgb_image="{rgb_image}"
-            point_cloud_topic_name="/wrist_camera/points"
-            rgb_camera_info_topic_name="/wrist_camera/camera_info"
-            rgb_image_topic_name="/wrist_camera/color"
-            rgb_camera_info="{rgb_camera_info}"
-          />
-          <Action
-            ID="TransformPointCloudFrame"
-            input_cloud="{point_cloud}"
-            target_frame="world"
-            output_cloud="{point_cloud}"
-          />
-          <Action
-            ID="RegisterPointClouds"
-            base_point_cloud="{model_point_cloud}"
-            target_point_cloud="{point_cloud}"
-            max_iterations="30"
-            max_correspondence_distance="0.02"
-            target_pose_in_base_frame="{model_to_real_pose}"
-          />
-          <Action
-            ID="TransformPointCloud"
-            input_cloud="{model_point_cloud}"
-            transform_pose="{model_to_real_pose}"
-            output_cloud="{aligned_model_cloud}"
-          />
+          <Action ID="TransformPointCloudFrame" input_cloud="{model_point_cloud}" target_frame="world" output_cloud="{model_point_cloud}"/>
+          <Action ID="GetSyncedImageAndPointCloud" point_cloud="{point_cloud}" rgb_image="{rgb_image}" point_cloud_topic_name="/wrist_camera/points" rgb_camera_info_topic_name="/wrist_camera/camera_info" rgb_image_topic_name="/wrist_camera/color" rgb_camera_info="{rgb_camera_info}"/>
+          <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" target_frame="world" output_cloud="{point_cloud}"/>
+          <Action ID="RegisterPointClouds" base_point_cloud="{model_point_cloud}" target_point_cloud="{point_cloud}" max_iterations="30" max_correspondence_distance="0.02" target_pose_in_base_frame="{model_to_real_pose}"/>
+          <Action ID="TransformPointCloud" input_cloud="{model_point_cloud}" transform_pose="{model_to_real_pose}" output_cloud="{aligned_model_cloud}"/>
           <Control ID="Fallback">
             <Decorator ID="Timeout" msec="100">
-              <Action
-                ID="PublishStaticFrame"
-                pose="{object_stamped_pose_estimate}"
-                child_frame_id="object_frame"
-                publish_rate="5"
-              />
+              <Action ID="PublishStaticFrame" pose="{object_stamped_pose_estimate}" child_frame_id="object_frame" publish_rate="5"/>
             </Decorator>
-            <Action ID="AlwaysSuccess" />
+            <Action ID="AlwaysSuccess"/>
           </Control>
-          <Action
-            ID="SendPointCloudToUI"
-            point_cloud="{aligned_model_cloud}"
-            pcd_topic="/pcd_pointcloud_captures"
-          />
-          <Action
-            ID="TransformPoseWithPose"
-            input_pose="{object_stamped_pose_estimate}"
-            transform_pose="{model_to_real_pose}"
-            output_pose="{model_world}"
-          />
+          <Action ID="SendPointCloudToUI" point_cloud="{aligned_model_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
+          <Action ID="TransformPoseWithPose" input_pose="{object_stamped_pose_estimate}" transform_pose="{model_to_real_pose}" output_pose="{model_world}"/>
           <Control ID="Fallback">
             <Decorator ID="Timeout" msec="1000">
-              <Action
-                ID="PublishStaticFrame"
-                pose="{model_world}"
-                child_frame_id="object_frame_new"
-                publish_rate="5"
-              />
+              <Action ID="PublishStaticFrame" pose="{model_world}" child_frame_id="object_frame_new" publish_rate="5"/>
             </Decorator>
-            <Action ID="AlwaysSuccess" />
+            <Action ID="AlwaysSuccess"/>
           </Control>
-          <Action
-            ID="TransformPoseFrame"
-            input_pose="{model_world}"
-            target_frame_id="object_frame_new"
-            output_pose="{model_object}"
-          />
-          <Action
-            ID="TransformPose"
-            input_pose="{model_object}"
-            translation_xyz="0.0;0.025;0.3"
-            quaternion_xyzw="-0.707107;0.707107;0.0;0.0"
-            output_pose="{hole_object}"
-          />
-          <Action
-            ID="TransformPoseFrame"
-            input_pose="{hole_object}"
-            target_frame_id="world"
-            output_pose="{hole_object}"
-          />
+          <Action ID="TransformPoseFrame" input_pose="{model_world}" target_frame_id="object_frame_new" output_pose="{model_object}"/>
+          <Action ID="TransformPose" input_pose="{model_object}" translation_xyz="0.0;0.025;0.3" quaternion_xyzw="-0.707107;0.707107;0.0;0.0" output_pose="{hole_object}"/>
+          <Action ID="TransformPoseFrame" input_pose="{hole_object}" target_frame_id="world" output_pose="{hole_object}"/>
           <Control ID="Fallback">
             <Decorator ID="Timeout" msec="1000">
-              <Action
-                ID="PublishStaticFrame"
-                pose="{hole_object}"
-                child_frame_id="object_hole"
-                publish_rate="5"
-              />
+              <Action ID="PublishStaticFrame" pose="{hole_object}" child_frame_id="object_hole" publish_rate="5"/>
             </Decorator>
-            <Action ID="AlwaysSuccess" />
+            <Action ID="AlwaysSuccess"/>
           </Control>
         </Control>
       </Control>
-      <Action
-        ID="InitializeMTCTask"
-        task_id=""
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        task="{mtc_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
-      <Action ID="SetupMTCPlanToPose" target_pose="{hole_object}" />
-      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
-      <Action ID="ExecuteMTCTask" solution="{mtc_solution}" />
-      <Action
-        ID="TransformPose"
-        input_pose="{hole_object}"
-        translation_xyz="0.0;0.0;-0.1"
-        quaternion_xyzw="0.0;1.0;0.0;0.0"
-        output_pose="{goal_pose}"
-      />
-      <Action
-        ID="InitializeMTCTask"
-        task_id=""
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        task="{mtc_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
-      <Action
-        ID="SetupMTCUpdateGroupCollisionRule"
-        group_name="gripper"
-        object_name="&lt;octomap&gt;"
-        allow_collision="true"
-        task="{mtc_task}"
-      />
-      <Action
-        ID="SetupMTCMoveAlongFrameAxis"
-        task="{mtc_task}"
-        hand_frame="grasp_link"
-        axis_frame="grasp_link"
-        axis_x="0.0"
-        axis_y="0.0"
-        axis_z="1.0"
-        max_distance="0.1"
-        min_distance="0"
-        planning_group_name="manipulator"
-        velocity_scale=".2"
-        acceleration_scale="1.0"
-      />
-      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
-      <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true" />
-      <Action ID="ExecuteMTCTask" solution="{mtc_solution}" />
-      <Action
-        ID="ActivateControllers"
-        controller_names="velocity_force_controller"
-      />
-      <Action
-        ID="CreateStampedTwist"
-        angular_velocity_xyz="0.0;0.0;0.0"
-        linear_velocity_xyz="0.0;0.0;0.0"
-        stamped_twist="{desired_twist}"
-      />
-      <Action
-        ID="CreateStampedWrench"
-        torque_xyz="0.0;0.0;0.0"
-        force_xyz="0.0;0.0;0.0"
-        stamped_wrench="{desired_wrench}"
-      />
+      <Action ID="InitializeMTCTask" task_id="" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{mtc_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
+      <Action ID="SetupMTCPlanToPose" target_pose="{hole_object}"/>
+      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{mtc_solution}"/>
+      <Action ID="TransformPose" input_pose="{hole_object}" translation_xyz="0.0;0.0;-0.1" quaternion_xyzw="0.0;1.0;0.0;0.0" output_pose="{goal_pose}"/>
+      <Action ID="InitializeMTCTask" task_id="" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{mtc_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
+      <Action ID="SetupMTCUpdateGroupCollisionRule" group_name="gripper" object_name="&lt;octomap&gt;" allow_collision="true" task="{mtc_task}"/>
+      <Action ID="SetupMTCMoveAlongFrameAxis" task="{mtc_task}" hand_frame="grasp_link" axis_frame="grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.1" min_distance="0" planning_group_name="manipulator" velocity_scale=".2" acceleration_scale="1.0"/>
+      <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}"/>
+      <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
+      <Action ID="ExecuteMTCTask" solution="{mtc_solution}"/>
+      <Action ID="ActivateControllers" controller_names="velocity_force_controller"/>
+      <Action ID="CreateStampedTwist" angular_velocity_xyz="0.0;0.0;0.0" linear_velocity_xyz="0.0;0.0;0.0" stamped_twist="{desired_twist}"/>
+      <Action ID="CreateStampedWrench" torque_xyz="0.0;0.0;0.0" force_xyz="0.0;0.0;0.0" stamped_wrench="{desired_wrench}"/>
       <Control ID="Parallel" success_count="1" failure_count="1">
-        <Action ID="PublishVelocityForceCommand" />
-        <SubTree ID="Close Gripper" _collapsed="true" />
+        <Action ID="PublishVelocityForceCommand"/>
+        <SubTree ID="Close Gripper" _collapsed="true"/>
       </Control>
       <Decorator ID="Delay" delay_msec="1000">
-        <Action
-          ID="ActivateControllers"
-          controller_names="joint_trajectory_controller"
-        />
+        <Action ID="ActivateControllers" controller_names="joint_trajectory_controller"/>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Compliant Grasp RAFTI VFC">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/demo_trajectory_stitching.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/demo_trajectory_stitching.xml
index 9e4d7cfe..97d48e6a 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/demo_trajectory_stitching.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/demo_trajectory_stitching.xml
@@ -1,135 +1,25 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Demo Trajectory Stitching">
   <!--//////////-->
-  <BehaviorTree
-    ID="Demo Trajectory Stitching"
-    _description="Take a pointcloud snapshot of the scene with a depth camera"
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Demo Trajectory Stitching" _description="Take a pointcloud snapshot of the scene with a depth camera" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        joint_group_name="manipulator"
-        planner_interface="moveit_default"
-        controller_names="/joint_trajectory_controller"
-        waypoint_name="Home"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-      />
-      <Action
-        ID="GetCurrentPlanningScene"
-        planning_scene_msg="{planning_scene}"
-      />
-      <Action
-        ID="GetRobotJointState"
-        planning_group_name="manipulator"
-        planning_scene="{planning_scene}"
-        joint_state="{start_joint_state}"
-      />
-      <Action
-        ID="UnpackJointStateMessage"
-        joint_state="{start_joint_state}"
-        effort="{start_effort}"
-        header="{start_header}"
-        name_="{joint_names}"
-        position="{start_position}"
-        velocity="{start_velocity}"
-      />
-      <Action
-        ID="CreateJointState"
-        joint_state_msg="{start_state}"
-        positions="{start_position}"
-        joint_names="{joint_names}"
-        velocities="{start_velocity}"
-      />
-      <Action
-        ID="CreateJointState"
-        positions="0.5;0.51;-3.025;-2.31;0.2;1.159;1.37"
-        joint_names="{joint_names}"
-        joint_state_msg="{target_state}"
-        velocities="0.0;0.0;0.0;0.0;0.0;0.0;0.0"
-      />
-      <Action
-        ID="GetRobotJointState"
-        planning_group_name="manipulator"
-        planning_scene="{planning_scene}"
-        joint_state="{joint_state}"
-      />
-      <Action
-        ID="CreateJointState"
-        positions="-0.5;-0.51;-3.025;-2.31;0.2;1.159;1.37"
-        joint_names="{joint_names}"
-        joint_state_msg="{stitch_target_state}"
-        velocities="0.0;0.0;0.0;0.0;0.0;0.0;0.0"
-      />
-      <Action
-        ID="GeneratePointToPointTrajectory"
-        jerk_scale_factor="0.5"
-        velocity_scale_factor="1.0"
-        acceleration_scale_factor="1.0"
-        planning_group_name="manipulator"
-        start_state="{start_state}"
-        start_time="0.000000"
-        target_state="{target_state}"
-        trajectory_sampling_rate="100"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
-      <Action
-        ID="GetTrajectoryStateAtTime"
-        time_from_reference="0.8"
-        joint_state="{stitch_state}"
-        from_start="true"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
-      <Action
-        ID="GeneratePointToPointTrajectory"
-        start_state="{stitch_state}"
-        target_state="{stitch_target_state}"
-        joint_trajectory_msg="{stitch_joint_trajectory_msg}"
-        velocity_scale_factor="1.0"
-        start_time="0.8"
-        acceleration_scale_factor="1.0"
-        jerk_scale_factor="0.5"
-        planning_group_name="manipulator"
-        trajectory_sampling_rate="100"
-      />
-      <Action
-        ID="SetAdmittanceParameters"
-        config_file_name="admittance_parameters_no_admittance.yaml"
-        admittance_parameters_msg="{admittance_parameters_msg}"
-      />
-      <Action
-        ID="ActivateControllers"
-        controller_names="/joint_trajectory_admittance_controller"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" joint_group_name="manipulator" planner_interface="moveit_default" controller_names="/joint_trajectory_controller" waypoint_name="Home" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0"/>
+      <Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene}"/>
+      <Action ID="GetRobotJointState" planning_group_name="manipulator" planning_scene="{planning_scene}" joint_state="{start_joint_state}"/>
+      <Action ID="UnpackJointStateMessage" joint_state="{start_joint_state}" effort="{start_effort}" header="{start_header}" name_="{joint_names}" position="{start_position}" velocity="{start_velocity}"/>
+      <Action ID="CreateJointState" joint_state_msg="{start_state}" positions="{start_position}" joint_names="{joint_names}" velocities="{start_velocity}"/>
+      <Action ID="CreateJointState" positions="0.5;0.51;-3.025;-2.31;0.2;1.159;1.37" joint_names="{joint_names}" joint_state_msg="{target_state}" velocities="0.0;0.0;0.0;0.0;0.0;0.0;0.0"/>
+      <Action ID="GetRobotJointState" planning_group_name="manipulator" planning_scene="{planning_scene}" joint_state="{joint_state}"/>
+      <Action ID="CreateJointState" positions="-0.5;-0.51;-3.025;-2.31;0.2;1.159;1.37" joint_names="{joint_names}" joint_state_msg="{stitch_target_state}" velocities="0.0;0.0;0.0;0.0;0.0;0.0;0.0"/>
+      <Action ID="GeneratePointToPointTrajectory" jerk_scale_factor="0.5" velocity_scale_factor="1.0" acceleration_scale_factor="1.0" planning_group_name="manipulator" start_state="{start_state}" start_time="0.000000" target_state="{target_state}" trajectory_sampling_rate="100" joint_trajectory_msg="{joint_trajectory_msg}"/>
+      <Action ID="GetTrajectoryStateAtTime" time_from_reference="0.8" joint_state="{stitch_state}" from_start="true" joint_trajectory_msg="{joint_trajectory_msg}"/>
+      <Action ID="GeneratePointToPointTrajectory" start_state="{stitch_state}" target_state="{stitch_target_state}" joint_trajectory_msg="{stitch_joint_trajectory_msg}" velocity_scale_factor="1.0" start_time="0.8" acceleration_scale_factor="1.0" jerk_scale_factor="0.5" planning_group_name="manipulator" trajectory_sampling_rate="100"/>
+      <Action ID="SetAdmittanceParameters" config_file_name="admittance_parameters_no_admittance.yaml" admittance_parameters_msg="{admittance_parameters_msg}"/>
+      <Action ID="ActivateControllers" controller_names="/joint_trajectory_admittance_controller"/>
       <Control ID="Parallel" failure_count="2" success_count="1">
-        <Action
-          ID="ExecuteTrajectoryWithAdmittance"
-          path_position_tolerance="0.2"
-          absolute_force_torque_threshold="45;45;45;10;10;10"
-          admittance_parameters_msg="{admittance_parameters_msg}"
-          controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
-          goal_duration_tolerance="-1.000000"
-          goal_position_tolerance="0.001000"
-          joint_trajectory_msg="{joint_trajectory_msg}"
-        />
+        <Action ID="ExecuteTrajectoryWithAdmittance" path_position_tolerance="0.2" absolute_force_torque_threshold="45;45;45;10;10;10" admittance_parameters_msg="{admittance_parameters_msg}" controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.001000" joint_trajectory_msg="{joint_trajectory_msg}"/>
         <Decorator ID="Delay" delay_msec="400">
-          <Action
-            ID="ExecuteTrajectoryWithAdmittance"
-            joint_trajectory_msg="{stitch_joint_trajectory_msg}"
-            absolute_force_torque_threshold="45;45;45;10;10;10"
-            goal_position_tolerance="0.001000"
-            path_position_tolerance="0.2"
-            admittance_parameters_msg="{admittance_parameters_msg}"
-            controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
-            goal_duration_tolerance="-1.000000"
-          />
+          <Action ID="ExecuteTrajectoryWithAdmittance" joint_trajectory_msg="{stitch_joint_trajectory_msg}" absolute_force_torque_threshold="45;45;45;10;10;10" goal_position_tolerance="0.001000" path_position_tolerance="0.2" admittance_parameters_msg="{admittance_parameters_msg}" controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000"/>
         </Decorator>
       </Control>
     </Control>
@@ -137,8 +27,8 @@
   <TreeNodesModel>
     <SubTree ID="Demo Trajectory Stitching">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/move_along_path.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/move_along_path.xml
index a2b5c521..d7c3445f 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/move_along_path.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/move_along_path.xml
@@ -1,119 +1,26 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move Along Path">
   <!--//////////-->
-  <BehaviorTree
-    ID="Move Along Path"
-    _description="Moves the robot tip on a rectangular path."
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Move Along Path" _description="Moves the robot tip on a rectangular path." _favorite="true" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="false"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        waypoint_name="View Satellite"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="false" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0" waypoint_name="View Satellite"/>
       <Control ID="Sequence">
-        <Action
-          ID="ActivateControllers"
-          controller_names="/joint_trajectory_controller"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.3;0.1;0.5"
-          orientation_xyzw="0.707;0.707;0.0;.0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.5;0.1;0.5"
-          orientation_xyzw="0.707;0.707;0.0;.0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.5;-0.1;0.5"
-          orientation_xyzw="0.707;0.707;0.0;.0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.3;-0.1;0.5"
-          orientation_xyzw="0.707;0.707;0.0;.0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
+        <Action ID="ActivateControllers" controller_names="/joint_trajectory_controller"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.3;0.1;0.5" orientation_xyzw="0.707;0.707;0.0;.0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.5;0.1;0.5" orientation_xyzw="0.707;0.707;0.0;.0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.5;-0.1;0.5" orientation_xyzw="0.707;0.707;0.0;.0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.3;-0.1;0.5" orientation_xyzw="0.707;0.707;0.0;.0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
       </Control>
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
-          <Action
-            ID="PlanCartesianPath"
-            path="{pose_stamped_vector}"
-            planning_group_name="manipulator"
-            tip_link="end_effector_link"
-            tip_offset="0.0;0.0;0.0;0.0;0.0;0.0"
-            position_only="false"
-            blending_radius="0.02"
-            velocity_scale_factor="0.5"
-            acceleration_scale_factor="0.5"
-            trajectory_sampling_rate="100"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-            debug_solution="{debug_solution}"
-            ik_cartesian_space_density="0.010000"
-            ik_joint_space_density="0.100000"
-          />
-          <Action
-            ID="GetCurrentPlanningScene"
-            planning_scene_msg="{planning_scene_msg}"
-          />
-          <Action
-            ID="ValidateTrajectory"
-            planning_scene_msg="{planning_scene_msg}"
-            planning_group_name="manipulator"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-            joint_space_step="0.05"
-            cartesian_space_step="0.02"
-            debug_solution="{debug_solution}"
-          />
-          <Action
-            ID="ExecuteFollowJointTrajectory"
-            execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-            goal_position_tolerance="0.000000"
-            goal_time_tolerance="0.000000"
-            goal_duration_tolerance="-1.000000"
-          />
+          <Action ID="PlanCartesianPath" path="{pose_stamped_vector}" planning_group_name="manipulator" tip_link="end_effector_link" tip_offset="0.0;0.0;0.0;0.0;0.0;0.0" position_only="false" blending_radius="0.02" velocity_scale_factor="0.5" acceleration_scale_factor="0.5" trajectory_sampling_rate="100" joint_trajectory_msg="{joint_trajectory_msg}" debug_solution="{debug_solution}" ik_cartesian_space_density="0.010000" ik_joint_space_density="0.100000"/>
+          <Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene_msg}"/>
+          <Action ID="ValidateTrajectory" planning_scene_msg="{planning_scene_msg}" planning_group_name="manipulator" joint_trajectory_msg="{joint_trajectory_msg}" joint_space_step="0.05" cartesian_space_step="0.02" debug_solution="{debug_solution}"/>
+          <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" joint_trajectory_msg="{joint_trajectory_msg}" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" goal_duration_tolerance="-1.000000"/>
         </Control>
       </Decorator>
     </Control>
@@ -121,7 +28,7 @@
   <TreeNodesModel>
     <SubTree ID="Move Along Path">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/move_along_path_admittance.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/move_along_path_admittance.xml
index 69a38a69..0e11117c 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/move_along_path_admittance.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/move_along_path_admittance.xml
@@ -1,111 +1,28 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move Along Path Admittance">
   <!--//////////-->
-  <BehaviorTree
-    ID="Move Along Path Admittance"
-    _description="Moves the robot tip on a rectangular path."
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Move Along Path Admittance" _description="Moves the robot tip on a rectangular path." _favorite="true" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <Control ID="Sequence">
-        <Action
-          ID="ActivateControllers"
-          controller_names="/joint_trajectory_admittance_controller"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.3;0.1;0.5"
-          orientation_xyzw="0.707;0.707;0.0;.0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.5;0.1;0.5"
-          orientation_xyzw="0.707;0.707;0.0;.0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.5;-0.1;0.5"
-          orientation_xyzw="0.707;0.707;0.0;.0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.3;-0.1;0.5"
-          orientation_xyzw="0.707;0.707;0.0;.0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
+        <Action ID="ActivateControllers" controller_names="/joint_trajectory_admittance_controller"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.3;0.1;0.5" orientation_xyzw="0.707;0.707;0.0;.0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.5;0.1;0.5" orientation_xyzw="0.707;0.707;0.0;.0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.5;-0.1;0.5" orientation_xyzw="0.707;0.707;0.0;.0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.3;-0.1;0.5" orientation_xyzw="0.707;0.707;0.0;.0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
       </Control>
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
-          <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true" />
-          <Action
-            ID="PlanCartesianPath"
-            path="{pose_stamped_vector}"
-            planning_group_name="manipulator"
-            tip_link="end_effector_link"
-            tip_offset="0.0;0.0;0.0;0.0;0.0;0.0"
-            position_only="false"
-            blending_radius="0.001"
-            velocity_scale_factor="0.2"
-            acceleration_scale_factor="0.5"
-            trajectory_sampling_rate="100"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-            debug_solution="{debug_solution}"
-          />
-          <Action
-            ID="GetCurrentPlanningScene"
-            planning_scene_msg="{planning_scene_msg}"
-          />
-          <Action
-            ID="ValidateTrajectory"
-            planning_scene_msg="{planning_scene_msg}"
-            planning_group_name="manipulator"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-            joint_space_step="0.05"
-            cartesian_space_step="0.02"
-            debug_solution="{debug_solution}"
-          />
-          <Action
-            ID="WaitForUserTrajectoryApproval"
-            solution="{debug_solution}"
-          />
-          <Action
-            ID="SetAdmittanceParameters"
-            config_file_name="jtac_parameters.yaml"
-          />
-          <Action
-            ID="ExecuteTrajectoryWithAdmittance"
-            path_position_tolerance="2.0"
-            goal_position_tolerance="0.1000"
-            controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
-          />
+          <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
+          <Action ID="PlanCartesianPath" path="{pose_stamped_vector}" planning_group_name="manipulator" tip_link="end_effector_link" tip_offset="0.0;0.0;0.0;0.0;0.0;0.0" position_only="false" blending_radius="0.001" velocity_scale_factor="0.2" acceleration_scale_factor="0.5" trajectory_sampling_rate="100" joint_trajectory_msg="{joint_trajectory_msg}" debug_solution="{debug_solution}"/>
+          <Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene_msg}"/>
+          <Action ID="ValidateTrajectory" planning_scene_msg="{planning_scene_msg}" planning_group_name="manipulator" joint_trajectory_msg="{joint_trajectory_msg}" joint_space_step="0.05" cartesian_space_step="0.02" debug_solution="{debug_solution}"/>
+          <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+          <Action ID="SetAdmittanceParameters" config_file_name="jtac_parameters.yaml"/>
+          <Action ID="ExecuteTrajectoryWithAdmittance" path_position_tolerance="2.0" goal_position_tolerance="0.1000" controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"/>
         </Control>
       </Decorator>
     </Control>
@@ -113,7 +30,7 @@
   <TreeNodesModel>
     <SubTree ID="Move Along Path Admittance">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/move_to_pose.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/move_to_pose.xml
index 8884959d..9f020ba8 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/move_to_pose.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/move_to_pose.xml
@@ -1,70 +1,32 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Pose">
-  <BehaviorTree
-    ID="Move to Pose"
-    _description="Uses inverse kinematics to move the robot to a set gripper position"
-  >
+  <BehaviorTree ID="Move to Pose" _description="Uses inverse kinematics to move the robot to a set gripper position">
     <Control ID="Fallback" name="root">
       <Control ID="Sequence">
-        <Action
-          ID="RetrievePoseParameter"
-          timeout_sec="-1"
-          pose="{target_pose}"
-        />
-        <Action
-          ID="InitializeMTCTask"
-          task_id="move_to_pose"
-          controller_names="/joint_trajectory_controller"
-          task="{move_to_pose_task}"
-        />
-        <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
-        <Action
-          ID="SetupMTCPlanToPose"
-          ik_frame="grasp_link"
-          planning_group_name="manipulator"
-          target_pose="{target_pose}"
-          task="{move_to_pose_task}"
-        />
-        <Action
-          ID="PlanMTCTask"
-          solution="{move_to_pose_solution}"
-          task="{move_to_pose_task}"
-        />
-        <SubTree
-          ID="Wait for Trajectory Approval if User Available"
-          solution="{move_to_pose_solution}"
-        />
-        <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
-        <Action
-          ID="PublishEmpty"
-          topic="/studio_ui/motion_ended"
-          queue_size="1"
-          use_best_effort="false"
-        />
+        <Action ID="RetrievePoseParameter" timeout_sec="-1" pose="{target_pose}"/>
+        <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
+        <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
+        <Action ID="SetupMTCPlanToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}"/>
+        <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+        <SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
+        <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}"/>
+        <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
       </Control>
       <Control ID="Sequence">
-        <Action
-          ID="PublishEmpty"
-          topic="/studio_ui/motion_ended"
-          queue_size="1"
-          use_best_effort="false"
-        />
-        <Action ID="AlwaysFailure" />
+        <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+        <Action ID="AlwaysFailure"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Pose">
-      <input_port name="use_all_planners" default="false" />
-      <input_port
-        name="controller_names"
-        default="/joint_trajectory_controller"
-      />
-      <input_port name="planning_group_name" default="manipulator" />
-      <input_port name="ik_frame" default="grasp_link" />
-      <input_port name="target_pose" default="{hole_object}" />
+      <input_port name="use_all_planners" default="false"/>
+      <input_port name="controller_names" default="/joint_trajectory_controller"/>
+      <input_port name="planning_group_name" default="manipulator"/>
+      <input_port name="ik_frame" default="grasp_link"/>
+      <input_port name="target_pose" default="{hole_object}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/open_gripper.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/open_gripper.xml
index 280fd7e6..be44be79 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/open_gripper.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/open_gripper.xml
@@ -1,19 +1,15 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Open Gripper">
   <!-- ////////// -->
   <BehaviorTree ID="Open Gripper" _description="Open the gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.0"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Open Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/re_zero_force_torque_sensors.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/re_zero_force_torque_sensors.xml
index 5fbbf52e..43f13d1f 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/re_zero_force_torque_sensors.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/re_zero_force_torque_sensors.xml
@@ -1,26 +1,18 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Re-Zero Force-Torque Sensors">
   <!--//////////-->
-  <BehaviorTree
-    ID="Re-Zero Force-Torque Sensors"
-    _description="Zero the force or torque sensors to mitigate accumulated drift error"
-    _favorite="false"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Re-Zero Force-Torque Sensors" _description="Zero the force or torque sensors to mitigate accumulated drift error" _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <!--Wrap in a ForceSuccess as Sim does not have the zero_ftsensor service-->
       <Decorator ID="ForceSuccess">
-        <Action
-          ID="CallTriggerService"
-          service_name="/io_and_status_controller/zero_fts"
-        />
+        <Action ID="CallTriggerService" service_name="/io_and_status_controller/zero_fts"/>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Re-Zero Force-Torque Sensors">
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/reactivate_gripper.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/reactivate_gripper.xml
index 88383610..493a70c1 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/reactivate_gripper.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/reactivate_gripper.xml
@@ -1,18 +1,15 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Reactivate Gripper">
   <!-- ////////// -->
   <BehaviorTree ID="Reactivate Gripper" _description="Reactivate the gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="CallTriggerService"
-        service_name="/robotiq_activation_controller/reactivate_gripper"
-      />
+      <Action ID="CallTriggerService" service_name="/robotiq_activation_controller/reactivate_gripper"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Reactivate Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/sample_april_tag.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/sample_april_tag.xml
index 0f26bbba..456b2d07 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/sample_april_tag.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/sample_april_tag.xml
@@ -1,63 +1,27 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Sample April Tag">
   <!--//////////-->
-  <BehaviorTree
-    ID="Sample April Tag"
-    _description="Samples the pose of an AprilTag and returns the average measurement."
-    _favorite="false"
-  >
+  <BehaviorTree ID="Sample April Tag" _description="Samples the pose of an AprilTag and returns the average measurement." _favorite="false">
     <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_camera/camera_info"
-          message_out="{camera_info}"
-        />
+        <Action ID="LoadObjectiveParameters" config_file_name="{apriltag_config}" parameters="{parameters}"/>
+        <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" message_out="{camera_info}"/>
         <Decorator ID="RetryUntilSuccessful" num_attempts="5">
           <Control ID="Sequence">
-            <Action
-              ID="GetImage"
-              topic_name="/wrist_camera/color"
-              message_out="{image}"
-            />
-            <Action
-              ID="DetectAprilTags"
-              image="{image}"
-              camera_info="{camera_info}"
-              parameters="{parameters}"
-              detections="{detections}"
-            />
-            <Action
-              ID="GetDetectionPose"
-              detections="{detections}"
-              target_label=""
-              detection_pose="{detection_pose}"
-              target_id="{tag_id}"
-            />
+            <Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}"/>
+            <Action ID="DetectAprilTags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/>
+            <Action ID="GetDetectionPose" detections="{detections}" target_label="" detection_pose="{detection_pose}" target_id="{tag_id}"/>
           </Control>
         </Decorator>
-        <Action
-          ID="AveragePoseStamped"
-          pose_sample="{detection_pose}"
-          avg_pose="{avg_pose}"
-          max_distance="{max_distance}"
-          max_rotation="{max_rotation}"
-          num_samples="{num_samples}"
-          run_continuously="false"
-        />
+        <Action ID="AveragePoseStamped" pose_sample="{detection_pose}" avg_pose="{avg_pose}" max_distance="{max_distance}" max_rotation="{max_rotation}" num_samples="{num_samples}" run_continuously="false"/>
       </Control>
     </Decorator>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Sample April Tag">
-      <input_port name="tag_id" default="-1" />
+      <input_port name="tag_id" default="-1"/>
       <MetadataFields>
-        <Metadata subcategory="Perception - Image Processing" />
+        <Metadata subcategory="Perception - Image Processing"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/take_snapshot.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/take_snapshot.xml
index 57a512d8..6861b36f 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/take_snapshot.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/take_snapshot.xml
@@ -1,22 +1,18 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Take Snapshot">
   <!--//////////-->
-  <BehaviorTree
-    ID="Take Snapshot"
-    _description="Take a pointcloud snapshot of the scene with a depth camera"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Take Snapshot" _description="Take a pointcloud snapshot of the scene with a depth camera" _favorite="true">
     <Control ID="Sequence">
       <!-- Clear out old snapshot data -->
-      <Action ID="ClearSnapshot" />
-      <Action ID="GetPointCloud" topic_name="/wrist_camera/points" />
-      <Action ID="SendPointCloudToUI" />
+      <Action ID="ClearSnapshot"/>
+      <Action ID="GetPointCloud" topic_name="/wrist_camera/points"/>
+      <Action ID="SendPointCloudToUI"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Take Snapshot">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/velocity_force_controller_zero.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/velocity_force_controller_zero.xml
index a7d29770..63263710 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/velocity_force_controller_zero.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/velocity_force_controller_zero.xml
@@ -1,42 +1,20 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Velocity Force Controller Zero">
   <!--//////////-->
-  <BehaviorTree
-    ID="Velocity Force Controller Zero"
-    _description="Publish a zero force command to the Velocity Force Controller"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Velocity Force Controller Zero" _description="Publish a zero force command to the Velocity Force Controller" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedTwist"
-        stamped_twist="{desired_twist}"
-        angular_velocity_xyz="0.0;0.0;0.0"
-        linear_velocity_xyz="0.0;0.0;0.0"
-      />
-      <Action
-        ID="ActivateControllers"
-        controller_names="/velocity_force_controller"
-      />
-      <Action
-        ID="CreateStampedWrench"
-        torque_xyz="0.0;0.0;0.0"
-        force_xyz="0.0;0.0;0.0"
-        stamped_wrench="{desired_wrench}"
-      />
+      <Action ID="CreateStampedTwist" stamped_twist="{desired_twist}" angular_velocity_xyz="0.0;0.0;0.0" linear_velocity_xyz="0.0;0.0;0.0"/>
+      <Action ID="ActivateControllers" controller_names="/velocity_force_controller"/>
+      <Action ID="CreateStampedWrench" torque_xyz="0.0;0.0;0.0" force_xyz="0.0;0.0;0.0" stamped_wrench="{desired_wrench}"/>
       <Decorator ID="KeepRunningUntilFailure">
-        <Action
-          ID="PublishVelocityForceCommand"
-          wrench_gain="0.1"
-          velocity_controlled_axes="0;0;0;0;0;0"
-          force_controlled_axes="1;1;1;0;0;0"
-        />
+        <Action ID="PublishVelocityForceCommand" wrench_gain="0.1" velocity_controlled_axes="0;0;0;0;0;0" force_controlled_axes="1;1;1;0;0;0"/>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Velocity Force Controller Zero">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/visual_servo_teach_reference.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/visual_servo_teach_reference.xml
index 678115b9..92e10233 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/visual_servo_teach_reference.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/visual_servo_teach_reference.xml
@@ -1,58 +1,19 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Visual Servo Teach Reference">
-  <BehaviorTree
-    ID="Visual Servo Teach Reference"
-    _description="Teach the pose reference used in `Visual Servo To Reference'"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Visual Servo Teach Reference" _description="Teach the pose reference used in `Visual Servo To Reference'" _favorite="true">
     <Control ID="Sequence" name="root">
-      <SubTree
-        ID="Sample April Tag"
-        _collapsed="false"
-        apriltag_config="51mm_apriltag_detection_config.yaml"
-        max_distance="0.02"
-        max_rotation="0.2"
-        avg_pose="{tag_pose}"
-        tag_id="0"
-        num_samples="1"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        position_xyz="0.0;0.0;0.0"
-        orientation_xyzw="0.0;0.0;0.0;1.0"
-        stamped_pose="{grasp_link_pose}"
-      />
-      <Action
-        ID="TransformPoseFrame"
-        input_pose="{tag_pose}"
-        target_frame_id="world"
-        output_pose="{tag_pose_world}"
-      />
-      <Action
-        ID="TransformPoseFrame"
-        input_pose="{grasp_link_pose}"
-        target_frame_id="world"
-        output_pose="{grasp_link_pose_world}"
-      />
-      <Action
-        ID="CalculatePoseOffset"
-        source_pose="{tag_pose_world}"
-        destination_pose="{grasp_link_pose_world}"
-        source_to_destination_pose="{tag_to_user_grasp_pose}"
-      />
-      <Action
-        ID="SavePoseToYaml"
-        yaml_filename="visual_servo_reference"
-        namespace="ServoReference"
-        message="{tag_to_user_grasp_pose}"
-      />
+      <SubTree ID="Sample April Tag" _collapsed="false" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}" tag_id="0" num_samples="1"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" position_xyz="0.0;0.0;0.0" orientation_xyzw="0.0;0.0;0.0;1.0" stamped_pose="{grasp_link_pose}"/>
+      <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
+      <Action ID="TransformPoseFrame" input_pose="{grasp_link_pose}" target_frame_id="world" output_pose="{grasp_link_pose_world}"/>
+      <Action ID="CalculatePoseOffset" source_pose="{tag_pose_world}" destination_pose="{grasp_link_pose_world}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
+      <Action ID="SavePoseToYaml" yaml_filename="visual_servo_reference" namespace="ServoReference" message="{tag_to_user_grasp_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Visual Servo Teach Reference">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/visual_servo_to_reference.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/visual_servo_to_reference.xml
index 9480ddfa..b4d7b4d9 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/visual_servo_to_reference.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/visual_servo_to_reference.xml
@@ -1,107 +1,38 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Visual Servo To Reference">
   <!--//////////-->
-  <BehaviorTree
-    ID="Visual Servo To Reference"
-    _description="Move in closed loop to a pose relative to an AprilTag"
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Visual Servo To Reference" _description="Move in closed loop to a pose relative to an AprilTag" _favorite="true" _hardcoded="false">
     <Control ID="Sequence">
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="visual_servo_reference.yaml"
-        parameters="{servo_reference_parameters}"
-      />
-      <Action ID="ActivateControllers" controller_names="servo_controller" />
+      <Action ID="LoadObjectiveParameters" config_file_name="visual_servo_reference.yaml" parameters="{servo_reference_parameters}"/>
+      <Action ID="ActivateControllers" controller_names="servo_controller"/>
       <Control ID="Sequence">
         <Decorator ID="RetryUntilSuccessful" num_attempts="10">
-          <SubTree
-            ID="Sample April Tag"
-            _collapsed="false"
-            num_samples="1"
-            tag_id="5"
-            apriltag_config="51mm_apriltag_detection_config.yaml"
-            max_distance="0.02"
-            max_rotation="0.2"
-            avg_pose="{tag_pose}"
-          />
+          <SubTree ID="Sample April Tag" _collapsed="false" num_samples="1" tag_id="5" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
         </Decorator>
-        <Action
-          ID="TransformPoseFrame"
-          input_pose="{tag_pose}"
-          target_frame_id="base_link"
-          output_pose="{tag_pose_base}"
-        />
-        <Action
-          ID="TransformPoseFromYaml"
-          input_pose="{tag_pose_base}"
-          parameter_namespace="ServoReference"
-          pose_parameters="{servo_reference_parameters}"
-          output_pose="{reference_pose}"
-        />
+        <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="base_link" output_pose="{tag_pose_base}"/>
+        <Action ID="TransformPoseFromYaml" input_pose="{tag_pose_base}" parameter_namespace="ServoReference" pose_parameters="{servo_reference_parameters}" output_pose="{reference_pose}"/>
       </Control>
       <Control ID="Parallel" failure_count="1" success_count="2">
         <Decorator ID="KeepRunningUntilFailure">
           <Control ID="Sequence">
-            <SubTree
-              ID="Sample April Tag"
-              _collapsed="true"
-              num_samples="1"
-              tag_id="5"
-              apriltag_config="51mm_apriltag_detection_config.yaml"
-              avg_pose="{tag_pose}"
-              max_distance="1.0"
-              max_rotation="0.5"
-            />
-            <Action
-              ID="TransformPoseFrame"
-              input_pose="{tag_pose}"
-              target_frame_id="base_link"
-              output_pose="{tag_pose_base}"
-            />
+            <SubTree ID="Sample April Tag" _collapsed="true" num_samples="1" tag_id="5" apriltag_config="51mm_apriltag_detection_config.yaml" avg_pose="{tag_pose}" max_distance="1.0" max_rotation="0.5"/>
+            <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="base_link" output_pose="{tag_pose_base}"/>
           </Control>
         </Decorator>
-        <Action
-          ID="AveragePoseStamped"
-          run_continuously="true"
-          num_samples="5"
-          max_distance="1.0"
-          pose_sample="{tag_pose_base}"
-          avg_pose="{tag_pose_avg}"
-          max_rotation="1.0"
-        />
+        <Action ID="AveragePoseStamped" run_continuously="true" num_samples="5" max_distance="1.0" pose_sample="{tag_pose_base}" avg_pose="{tag_pose_avg}" max_rotation="1.0"/>
         <Decorator ID="KeepRunningUntilFailure">
           <Control ID="Sequence">
-            <Action
-              ID="TransformPoseFromYaml"
-              input_pose="{tag_pose_avg}"
-              parameter_namespace="ServoReference"
-              pose_parameters="{servo_reference_parameters}"
-              output_pose="{reference_pose}"
-            />
+            <Action ID="TransformPoseFromYaml" input_pose="{tag_pose_avg}" parameter_namespace="ServoReference" pose_parameters="{servo_reference_parameters}" output_pose="{reference_pose}"/>
           </Control>
         </Decorator>
-        <Action
-          ID="ServoTowardsPose"
-          planning_group_name="manipulator"
-          target_pose="{reference_pose}"
-          rotational_gain="0.1"
-          max_translational_vel="0.75"
-          max_rotational_vel="0.3"
-          publish_rate="20"
-          exit_threshold_translation="0.000000005"
-          exit_threshold_rotation="0.00000000005"
-          exit_threshold_time="3000"
-          translational_gain="1.0"
-        />
+        <Action ID="ServoTowardsPose" planning_group_name="manipulator" target_pose="{reference_pose}" rotational_gain="0.1" max_translational_vel="0.75" max_rotational_vel="0.3" publish_rate="20" exit_threshold_translation="0.000000005" exit_threshold_rotation="0.00000000005" exit_threshold_time="3000" translational_gain="1.0"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Visual Servo To Reference">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/write_picknik.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/write_picknik.xml
index e7b003c1..b5482316 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/write_picknik.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/write_picknik.xml
@@ -1,64 +1,19 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Write PickNik">
   <!--//////////-->
-  <BehaviorTree
-    ID="Write PickNik"
-    _description=""
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Write PickNik" _description="" _favorite="true" _hardcoded="false">
     <Control ID="Sequence">
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="base_link"
-        position_xyz="0.4;0.3;0.09"
-        orientation_xyzw="0.7073883;-0.7068252;0.0000327;0.0000328"
-        stamped_pose="{stamped_pose}"
-      />
+      <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.4;0.3;0.09" orientation_xyzw="0.7073883;-0.7068252;0.0000327;0.0000328" stamped_pose="{stamped_pose}"/>
       <Control ID="Parallel" success_count="1" failure_count="1">
-        <Action
-          ID="PublishStaticFrame"
-          pose="{stamped_pose}"
-          child_frame_id="local"
-          publish_rate="50"
-        />
+        <Action ID="PublishStaticFrame" pose="{stamped_pose}" child_frame_id="local" publish_rate="50"/>
         <Control ID="Sequence" name="TopLevelSequence">
-          <Action
-            ID="ActivateControllers"
-            controller_names="joint_trajectory_controller"
-          />
-          <Action
-            ID="LoadPoseStampedVectorFromYaml"
-            file_path="write_picknik_waypoints.yaml"
-            output="{pose_stamped_msgs}"
-          />
+          <Action ID="ActivateControllers" controller_names="joint_trajectory_controller"/>
+          <Action ID="LoadPoseStampedVectorFromYaml" file_path="write_picknik_waypoints.yaml" output="{pose_stamped_msgs}"/>
           <Control ID="Fallback">
-            <Action
-              ID="PlanCartesianPath"
-              path="{pose_stamped_msgs}"
-              planning_group_name="manipulator"
-              tip_link="grasp_link"
-              tip_offset="0.0;0.0;0.0;0.0;0.0;0.0"
-              position_only="false"
-              blending_radius="0.001"
-              velocity_scale_factor="0.5"
-              acceleration_scale_factor="0.5"
-              trajectory_sampling_rate="100"
-              joint_trajectory_msg="{joint_trajectory_msg}"
-              debug_solution="{debug_solution}"
-            />
-            <Action
-              ID="WaitForUserTrajectoryApproval"
-              solution="{debug_solution}"
-            />
+            <Action ID="PlanCartesianPath" path="{pose_stamped_msgs}" planning_group_name="manipulator" tip_link="grasp_link" tip_offset="0.0;0.0;0.0;0.0;0.0;0.0" position_only="false" blending_radius="0.001" velocity_scale_factor="0.5" acceleration_scale_factor="0.5" trajectory_sampling_rate="100" joint_trajectory_msg="{joint_trajectory_msg}" debug_solution="{debug_solution}"/>
+            <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
           </Control>
-          <Action
-            ID="ExecuteFollowJointTrajectory"
-            execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-            goal_position_tolerance="0.000000"
-            goal_time_tolerance="0.000000"
-          />
+          <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" joint_trajectory_msg="{joint_trajectory_msg}" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000"/>
         </Control>
       </Control>
     </Control>
@@ -66,7 +21,7 @@
   <TreeNodesModel>
     <SubTree ID="Write PickNik">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/kinova_sim/package.xml b/src/moveit_pro_kinova_configs/kinova_sim/package.xml
index cca74f3b..6efab469 100644
--- a/src/moveit_pro_kinova_configs/kinova_sim/package.xml
+++ b/src/moveit_pro_kinova_configs/kinova_sim/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>kinova_sim</name>
   <version>6.5.0</version>
diff --git a/src/moveit_pro_kinova_configs/moveit_studio_kinova_pstop_manager/package.xml b/src/moveit_pro_kinova_configs/moveit_studio_kinova_pstop_manager/package.xml
index b5031ef5..a3d8c9c9 100644
--- a/src/moveit_pro_kinova_configs/moveit_studio_kinova_pstop_manager/package.xml
+++ b/src/moveit_pro_kinova_configs/moveit_studio_kinova_pstop_manager/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>moveit_studio_kinova_pstop_manager</name>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim/description/mujoco/gen3_7dof_assets.xml b/src/moveit_pro_kinova_configs/space_satellite_sim/description/mujoco/gen3_7dof_assets.xml
index c02ef8cd..a3bdfd76 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim/description/mujoco/gen3_7dof_assets.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim/description/mujoco/gen3_7dof_assets.xml
@@ -1,255 +1,122 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujocoinclude>
   <default>
     <!-- kinova defaults -->
     <default class="kinova">
-      <material specular="0.5" shininess="0.25" />
-      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973" />
-      <general
-        dyntype="none"
-        biastype="affine"
-        ctrlrange="-6.28 6.28"
-        forcerange="-87 87"
-      />
+      <material specular="0.5" shininess="0.25"/>
+      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
+      <general dyntype="none" biastype="affine" ctrlrange="-6.28 6.28" forcerange="-87 87"/>
       <default class="infiniteJoints">
-        <joint limited="false" />
+        <joint limited="false"/>
       </default>
       <default class="limitedJoints">
-        <joint limited="true" />
+        <joint limited="true"/>
       </default>
-      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4" />
+      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
     </default>
     <!-- robotiq 2f85 defaults -->
     <default class="2f85">
-      <mesh scale="0.001 0.001 0.001" />
-      <general biastype="affine" />
+      <mesh scale="0.001 0.001 0.001"/>
+      <general biastype="affine"/>
 
-      <joint axis="1 0 0" />
+      <joint axis="1 0 0"/>
       <default class="knuckle_link">
-        <joint
-          range="0 0.8"
-          armature="0.005"
-          damping="0.1"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="0 0.8" armature="0.005" damping="0.1" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
       <default class="finger_tip_link">
-        <joint
-          range="-0.872664 0.872664"
-          armature="0.001"
-          pos="0 -0.018 0.0065"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="-0.872664 0.872664" armature="0.001" pos="0 -0.018 0.0065" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
       <default class="inner_knuckle_link">
-        <joint
-          range="-0.29670597283 0.8"
-          armature="0.001"
-          stiffness="0.05"
-          springref="2.62"
-          damping="0.00125"
-        />
+        <joint range="-0.29670597283 0.8" armature="0.001" stiffness="0.05" springref="2.62" damping="0.00125"/>
       </default>
       <default class="finger_link">
-        <joint
-          range="-1.57 0"
-          armature="0.001"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="-1.57 0" armature="0.001" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
 
       <default class="visual">
-        <geom type="mesh" contype="0" conaffinity="0" group="2" />
+        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
       </default>
       <default class="collision">
-        <geom type="mesh" group="3" condim="6" />
+        <geom type="mesh" group="3" condim="6"/>
         <default class="pad_box1">
-          <geom
-            mass="0"
-            type="box"
-            pos="0 -0.0026 0.028125"
-            size="0.011 0.004 0.009375"
-            friction="0.7"
-            solimp="0.95 0.99 0.001"
-            solref="0.004 1"
-            priority="1"
-            rgba="0.55 0.55 0.55 1"
-          />
+          <geom mass="0" type="box" pos="0 -0.0026 0.028125" size="0.011 0.004 0.009375" friction="0.7" solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.55 0.55 0.55 1"/>
         </default>
         <default class="pad_box2">
-          <geom
-            mass="0"
-            type="box"
-            pos="0 -0.0026 0.009375"
-            size="0.011 0.004 0.009375"
-            friction="0.6"
-            solimp="0.95 0.99 0.001"
-            solref="0.004 1"
-            priority="1"
-            rgba="0.45 0.45 0.45 1"
-          />
+          <geom mass="0" type="box" pos="0 -0.0026 0.009375" size="0.011 0.004 0.009375" friction="0.6" solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.45 0.45 0.45 1"/>
         </default>
       </default>
     </default>
   </default>
 
   <asset>
-    <material name="metal" rgba="0.58 0.58 0.58 1" />
-    <material name="silicone" rgba="0.1882 0.1882 0.1882 1" />
-    <material name="white" rgba="1.0 1.0 1.0 1" />
-    <material name="gray" rgba="0.4627 0.4627 0.4627 1" />
-    <material name="black" rgba="0.149 0.149 0.149 1" />
+    <material name="metal" rgba="0.58 0.58 0.58 1"/>
+    <material name="silicone" rgba="0.1882 0.1882 0.1882 1"/>
+    <material name="white" rgba="1.0 1.0 1.0 1"/>
+    <material name="gray" rgba="0.4627 0.4627 0.4627 1"/>
+    <material name="black" rgba="0.149 0.149 0.149 1"/>
 
     <!-- Kinova collision meshes -->
-    <mesh name="base_link_c" file="kinova/base_link.STL" />
-    <mesh name="shoulder_link_c" file="kinova/shoulder_link.STL" />
-    <mesh name="half_arm_1_link_c" file="kinova/half_arm_1_link.STL" />
-    <mesh name="half_arm_2_link_c" file="kinova/half_arm_2_link.STL" />
-    <mesh name="forearm_link_c" file="kinova/forearm_link.STL" />
-    <mesh
-      name="spherical_wrist_1_link_c"
-      file="kinova/spherical_wrist_1_link.STL"
-    />
-    <mesh
-      name="spherical_wrist_2_link_c"
-      file="kinova/spherical_wrist_2_link.STL"
-    />
-    <mesh
-      name="bracelet_with_vision_link_c"
-      file="kinova/bracelet_with_vision_link.STL"
-    />
+    <mesh name="base_link_c" file="kinova/base_link.STL"/>
+    <mesh name="shoulder_link_c" file="kinova/shoulder_link.STL"/>
+    <mesh name="half_arm_1_link_c" file="kinova/half_arm_1_link.STL"/>
+    <mesh name="half_arm_2_link_c" file="kinova/half_arm_2_link.STL"/>
+    <mesh name="forearm_link_c" file="kinova/forearm_link.STL"/>
+    <mesh name="spherical_wrist_1_link_c" file="kinova/spherical_wrist_1_link.STL"/>
+    <mesh name="spherical_wrist_2_link_c" file="kinova/spherical_wrist_2_link.STL"/>
+    <mesh name="bracelet_with_vision_link_c" file="kinova/bracelet_with_vision_link.STL"/>
     <!-- Kinova visual meshes -->
-    <mesh name="base_link" file="kinova/base_link.obj" />
-    <mesh name="shoulder_link" file="kinova/shoulder_link.obj" />
-    <mesh name="half_arm_1_link" file="kinova/half_arm_1_link.obj" />
-    <mesh name="half_arm_2_link" file="kinova/half_arm_2_link.obj" />
-    <mesh name="forearm_link" file="kinova/forearm_link.obj" />
-    <mesh
-      name="spherical_wrist_1_link"
-      file="kinova/spherical_wrist_1_link.obj"
-    />
-    <mesh
-      name="spherical_wrist_2_link"
-      file="kinova/spherical_wrist_2_link.obj"
-    />
-    <mesh
-      name="bracelet_with_vision_link"
-      file="kinova/bracelet_with_vision_link.obj"
-    />
+    <mesh name="base_link" file="kinova/base_link.obj"/>
+    <mesh name="shoulder_link" file="kinova/shoulder_link.obj"/>
+    <mesh name="half_arm_1_link" file="kinova/half_arm_1_link.obj"/>
+    <mesh name="half_arm_2_link" file="kinova/half_arm_2_link.obj"/>
+    <mesh name="forearm_link" file="kinova/forearm_link.obj"/>
+    <mesh name="spherical_wrist_1_link" file="kinova/spherical_wrist_1_link.obj"/>
+    <mesh name="spherical_wrist_2_link" file="kinova/spherical_wrist_2_link.obj"/>
+    <mesh name="bracelet_with_vision_link" file="kinova/bracelet_with_vision_link.obj"/>
     <!-- Robotiq 2f85 meshes -->
-    <mesh name="robotiq_85_base" class="2f85" file="robotiq_2f85/base.stl" />
-    <mesh
-      name="robotiq_85_knuckle_link"
-      class="2f85"
-      file="robotiq_2f85/knuckle_link.stl"
-    />
-    <mesh
-      name="robotiq_85_inner_knuckle_link"
-      class="2f85"
-      file="robotiq_2f85/inner_knuckle_link.stl"
-    />
-    <mesh
-      name="robotiq_85_finger_link"
-      class="2f85"
-      file="robotiq_2f85/finger_link.stl"
-    />
-    <mesh
-      name="robotiq_85_finger_tip_link"
-      class="2f85"
-      file="robotiq_2f85/finger_tip_link.stl"
-    />
-    <mesh name="robotiq_85_pad" class="2f85" file="robotiq_2f85/pad.stl" />
-    <mesh
-      name="robotiq_85_silicone_pad"
-      class="2f85"
-      file="robotiq_2f85/silicone_pad.stl"
-    />
-    <mesh name="rafti_finger" file="rafti/rafti_finger.obj" />
-    <mesh
-      name="rafti_finger_collision_1"
-      file="rafti/rafti_finger_collision_1.obj"
-    />
-    <mesh
-      name="rafti_finger_collision_2"
-      file="rafti/rafti_finger_collision_2.obj"
-    />
-    <mesh
-      name="rafti_finger_collision_3"
-      file="rafti/rafti_finger_collision_3.obj"
-    />
-    <mesh
-      name="rafti_finger_collision_4"
-      file="rafti/rafti_finger_collision_4.obj"
-    />
+    <mesh name="robotiq_85_base" class="2f85" file="robotiq_2f85/base.stl"/>
+    <mesh name="robotiq_85_knuckle_link" class="2f85" file="robotiq_2f85/knuckle_link.stl"/>
+    <mesh name="robotiq_85_inner_knuckle_link" class="2f85" file="robotiq_2f85/inner_knuckle_link.stl"/>
+    <mesh name="robotiq_85_finger_link" class="2f85" file="robotiq_2f85/finger_link.stl"/>
+    <mesh name="robotiq_85_finger_tip_link" class="2f85" file="robotiq_2f85/finger_tip_link.stl"/>
+    <mesh name="robotiq_85_pad" class="2f85" file="robotiq_2f85/pad.stl"/>
+    <mesh name="robotiq_85_silicone_pad" class="2f85" file="robotiq_2f85/silicone_pad.stl"/>
+    <mesh name="rafti_finger" file="rafti/rafti_finger.obj"/>
+    <mesh name="rafti_finger_collision_1" file="rafti/rafti_finger_collision_1.obj"/>
+    <mesh name="rafti_finger_collision_2" file="rafti/rafti_finger_collision_2.obj"/>
+    <mesh name="rafti_finger_collision_3" file="rafti/rafti_finger_collision_3.obj"/>
+    <mesh name="rafti_finger_collision_4" file="rafti/rafti_finger_collision_4.obj"/>
   </asset>
 
   <contact>
-    <exclude body1="base_link" body2="shoulder_link" />
+    <exclude body1="base_link" body2="shoulder_link"/>
   </contact>
 
   <actuator>
-    <position class="kinova" name="joint_1" joint="joint_1" kp="800" kv="260" />
-    <position class="kinova" name="joint_2" joint="joint_2" kp="800" kv="260" />
-    <position class="kinova" name="joint_3" joint="joint_3" kp="260" kv="86" />
-    <position class="kinova" name="joint_4" joint="joint_4" kp="260" kv="86" />
-    <position class="kinova" name="joint_5" joint="joint_5" kp="130" kv="45" />
-    <position class="kinova" name="joint_6" joint="joint_6" kp="130" kv="45" />
-    <position class="kinova" name="joint_7" joint="joint_7" kp="130" kv="45" />
+    <position class="kinova" name="joint_1" joint="joint_1" kp="800" kv="260"/>
+    <position class="kinova" name="joint_2" joint="joint_2" kp="800" kv="260"/>
+    <position class="kinova" name="joint_3" joint="joint_3" kp="260" kv="86"/>
+    <position class="kinova" name="joint_4" joint="joint_4" kp="260" kv="86"/>
+    <position class="kinova" name="joint_5" joint="joint_5" kp="130" kv="45"/>
+    <position class="kinova" name="joint_6" joint="joint_6" kp="130" kv="45"/>
+    <position class="kinova" name="joint_7" joint="joint_7" kp="130" kv="45"/>
   </actuator>
 
   <!-- Robotiq gripper contact -->
   <contact>
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_left_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_right_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_left_inner_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_right_inner_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_right_finger_link"
-      body2="robotiq_85_right_finger_tip_link"
-    />
-    <exclude
-      body1="robotiq_85_left_finger_link"
-      body2="robotiq_85_left_finger_tip_link"
-    />
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_left_knuckle_link"/>
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_right_knuckle_link"/>
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_left_inner_knuckle_link"/>
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_right_inner_knuckle_link"/>
+    <exclude body1="robotiq_85_right_finger_link" body2="robotiq_85_right_finger_tip_link"/>
+    <exclude body1="robotiq_85_left_finger_link" body2="robotiq_85_left_finger_tip_link"/>
     <!--                              TODO uncomment to enable RAFTI fingers -->
-    <exclude
-      body1="right_rafti_finger_link"
-      body2="robotiq_85_right_finger_tip_link"
-    />
-    <exclude
-      body1="right_rafti_finger_link"
-      body2="robotiq_85_right_inner_knuckle_link"
-    />
-    <exclude
-      body1="right_rafti_finger_link"
-      body2="robotiq_85_right_knuckle_link"
-    />
-    <exclude
-      body1="left_rafti_finger_link"
-      body2="robotiq_85_left_finger_tip_link"
-    />
-    <exclude
-      body1="left_rafti_finger_link"
-      body2="robotiq_85_left_inner_knuckle_link"
-    />
-    <exclude
-      body1="left_rafti_finger_link"
-      body2="robotiq_85_left_knuckle_link"
-    />
+    <exclude body1="right_rafti_finger_link" body2="robotiq_85_right_finger_tip_link"/>
+    <exclude body1="right_rafti_finger_link" body2="robotiq_85_right_inner_knuckle_link"/>
+    <exclude body1="right_rafti_finger_link" body2="robotiq_85_right_knuckle_link"/>
+    <exclude body1="left_rafti_finger_link" body2="robotiq_85_left_finger_tip_link"/>
+    <exclude body1="left_rafti_finger_link" body2="robotiq_85_left_inner_knuckle_link"/>
+    <exclude body1="left_rafti_finger_link" body2="robotiq_85_left_knuckle_link"/>
   </contact>
 
   <!--
@@ -260,49 +127,23 @@
   -->
   <tendon>
     <fixed name="split">
-      <joint joint="robotiq_85_right_knuckle_joint" coef="0.5" />
-      <joint joint="robotiq_85_left_knuckle_joint" coef="0.5" />
+      <joint joint="robotiq_85_right_knuckle_joint" coef="0.5"/>
+      <joint joint="robotiq_85_left_knuckle_joint" coef="0.5"/>
     </fixed>
   </tendon>
 
   <equality>
-    <connect
-      anchor="0 0 0"
-      body1="robotiq_85_right_finger_tip_link"
-      body2="robotiq_85_right_finger_link"
-      solimp="0.95 0.99 0.001"
-      solref="0.005 1"
-    />
-    <connect
-      anchor="0 0 0"
-      body1="robotiq_85_left_finger_tip_link"
-      body2="robotiq_85_left_finger_link"
-      solimp="0.95 0.99 0.001"
-      solref="0.005 1"
-    />
-    <joint
-      joint1="robotiq_85_left_knuckle_joint"
-      joint2="robotiq_85_right_knuckle_joint"
-      polycoef="0 1 0 0 0"
-      solimp="0.95 0.99 0.001"
-      solref="0.005 1"
-    />
+    <connect anchor="0 0 0" body1="robotiq_85_right_finger_tip_link" body2="robotiq_85_right_finger_link" solimp="0.95 0.99 0.001" solref="0.005 1"/>
+    <connect anchor="0 0 0" body1="robotiq_85_left_finger_tip_link" body2="robotiq_85_left_finger_link" solimp="0.95 0.99 0.001" solref="0.005 1"/>
+    <joint joint1="robotiq_85_left_knuckle_joint" joint2="robotiq_85_right_knuckle_joint" polycoef="0 1 0 0 0" solimp="0.95 0.99 0.001" solref="0.005 1"/>
   </equality>
 
   <actuator>
-    <position
-      class="2f85"
-      name="robotiq_85_left_knuckle_joint"
-      tendon="split"
-      forcerange="-5 5"
-      ctrlrange="0 .8"
-      kp="100"
-      kv="100"
-    />
+    <position class="2f85" name="robotiq_85_left_knuckle_joint" tendon="split" forcerange="-5 5" ctrlrange="0 .8" kp="100" kv="100"/>
   </actuator>
 
   <sensor>
-    <force site="fts" />
-    <torque site="fts" />
+    <force site="fts"/>
+    <torque site="fts"/>
   </sensor>
 </mujocoinclude>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim/description/mujoco/gen3_7dof_body.xml b/src/moveit_pro_kinova_configs/space_satellite_sim/description/mujoco/gen3_7dof_body.xml
index 275b12a7..d812b4a8 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim/description/mujoco/gen3_7dof_body.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim/description/mujoco/gen3_7dof_body.xml
@@ -1,505 +1,135 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujocoinclude>
   <body name="base_link" gravcomp="1" pos="0 0 0.02" quat="1 0 0 0">
-    <inertial
-      pos="-0.000648 -0.000166 0.084487"
-      quat="1 0 0 0"
-      mass="1.697"
-      diaginertia="0.004622 0.004495 0.002079"
-    />
-    <geom class="visual" mesh="base_link" material="white" />
-    <geom class="collision" mesh="base_link_c" />
-    <body
-      name="shoulder_link"
-      gravcomp="1"
-      pos="0 0 0.15643"
-      quat="-3.67321e-06 1 0 0"
-    >
-      <inertial
-        pos="-2.3e-05 -0.010364 -0.07336"
-        quat="0.707051 0.0451246 -0.0453544 0.704263"
-        mass="1.3773"
-        diaginertia="0.00488868 0.00457 0.00135132"
-      />
-      <joint class="infiniteJoints" name="joint_1" armature="0.0000015" />
-      <geom class="visual" mesh="shoulder_link" material="white" />
-      <geom class="collision" mesh="shoulder_link_c" />
-      <body
-        name="half_arm_1_link"
-        gravcomp="1"
-        pos="0 0.005375 -0.12838"
-        quat="0.707105 0.707108 0 0"
-      >
-        <inertial
-          pos="-4.4e-05 -0.09958 -0.013278"
-          quat="0.482348 0.516286 -0.516862 0.483366"
-          mass="1.1636"
-          diaginertia="0.0113017 0.011088 0.00102532"
-        />
-        <joint
-          class="limitedJoints"
-          name="joint_2"
-          range="-2.41 2.41"
-          armature="0.0000015"
-        />
-        <geom class="visual" mesh="half_arm_1_link" material="white" />
-        <geom class="collision" mesh="half_arm_1_link_c" />
-        <body
-          name="half_arm_2_link"
-          gravcomp="1"
-          pos="0 -0.21038 -0.006375"
-          quat="0.707105 -0.707108 0 0"
-        >
-          <inertial
-            pos="-4.4e-05 -0.006641 -0.117892"
-            quat="0.706144 0.0213722 -0.0209128 0.707437"
-            mass="1.1636"
-            diaginertia="0.0111633 0.010932 0.00100671"
-          />
-          <joint class="infiniteJoints" name="joint_3" armature="0.0000015" />
-          <geom class="visual" mesh="half_arm_2_link" material="white" />
-          <geom class="collision" mesh="half_arm_2_link_c" />
-          <body
-            name="forearm_link"
-            gravcomp="1"
-            pos="0 0.006375 -0.21038"
-            quat="0.707105 0.707108 0 0"
-          >
-            <inertial
-              pos="-1.8e-05 -0.075478 -0.015006"
-              quat="0.483678 0.515961 -0.515859 0.483455"
-              mass="0.9302"
-              diaginertia="0.00834839 0.008147 0.000598606"
-            />
-            <joint
-              class="limitedJoints"
-              name="joint_4"
-              range="-2.66 2.66"
-              armature="0.0000015"
-            />
-            <geom class="visual" mesh="forearm_link" material="white" />
-            <geom class="collision" mesh="forearm_link_c" />
-            <body
-              name="spherical_wrist_1_link"
-              gravcomp="1"
-              pos="0 -0.20843 -0.006375"
-              quat="0.707105 -0.707108 0 0"
-            >
-              <inertial
-                pos="1e-06 -0.009432 -0.063883"
-                quat="0.703558 0.0707492 -0.0707492 0.703558"
-                mass="0.6781"
-                diaginertia="0.00165901 0.001596 0.000346988"
-              />
-              <joint
-                class="infiniteJoints"
-                name="joint_5"
-                armature="0.000001928"
-              />
-              <geom
-                class="visual"
-                mesh="spherical_wrist_1_link"
-                material="white"
-              />
-              <geom class="collision" mesh="spherical_wrist_1_link_c" />
-              <body
-                name="spherical_wrist_2_link"
-                gravcomp="1"
-                pos="0 0.00017505 -0.10593"
-                quat="0.707105 0.707108 0 0"
-              >
-                <inertial
-                  pos="1e-06 -0.045483 -0.00965"
-                  quat="0.44426 0.550121 -0.550121 0.44426"
-                  mass="0.6781"
-                  diaginertia="0.00170087 0.001641 0.00035013"
-                />
-                <joint
-                  class="limitedJoints"
-                  name="joint_6"
-                  range="-2.23 2.23"
-                  armature="0.000001928"
-                />
-                <geom
-                  class="visual"
-                  mesh="spherical_wrist_2_link"
-                  material="white"
-                />
-                <geom class="collision" mesh="spherical_wrist_2_link_c" />
-                <body
-                  name="bracelet_link"
-                  gravcomp="1"
-                  pos="0 -0.10593 -0.00017505"
-                  quat="0.707105 -0.707108 0 0"
-                >
-                  <camera
-                    name="wrist_camera"
-                    pos="0.0 -0.056 -0.058"
-                    fovy="58"
-                    mode="fixed"
-                    resolution="640 480"
-                    quat="0.0 0.0 0.0 1.0"
-                  />
-                  <site
-                    name="wrist_camera_optical_frame"
-                    pos="0.0 -0.056 -0.058"
-                    quat="0.0 0.0 1.0 0.0"
-                  />
-                  <inertial
-                    pos="-9.3e-05 0.000132 -0.022905"
-                    quat="-0.0368025 0.719402 -0.0444884 0.69219"
-                    mass="0.364"
-                    diaginertia="0.00024027 0.000222769 0.000213961"
-                  />
-                  <joint
-                    class="infiniteJoints"
-                    name="joint_7"
-                    armature="0.000001928"
-                  />
-                  <geom
-                    class="visual"
-                    mesh="bracelet_with_vision_link"
-                    material="white"
-                  />
-                  <geom class="collision" mesh="bracelet_with_vision_link_c" />
+    <inertial pos="-0.000648 -0.000166 0.084487" quat="1 0 0 0" mass="1.697" diaginertia="0.004622 0.004495 0.002079"/>
+    <geom class="visual" mesh="base_link" material="white"/>
+    <geom class="collision" mesh="base_link_c"/>
+    <body name="shoulder_link" gravcomp="1" pos="0 0 0.15643" quat="-3.67321e-06 1 0 0">
+      <inertial pos="-2.3e-05 -0.010364 -0.07336" quat="0.707051 0.0451246 -0.0453544 0.704263" mass="1.3773" diaginertia="0.00488868 0.00457 0.00135132"/>
+      <joint class="infiniteJoints" name="joint_1" armature="0.0000015"/>
+      <geom class="visual" mesh="shoulder_link" material="white"/>
+      <geom class="collision" mesh="shoulder_link_c"/>
+      <body name="half_arm_1_link" gravcomp="1" pos="0 0.005375 -0.12838" quat="0.707105 0.707108 0 0">
+        <inertial pos="-4.4e-05 -0.09958 -0.013278" quat="0.482348 0.516286 -0.516862 0.483366" mass="1.1636" diaginertia="0.0113017 0.011088 0.00102532"/>
+        <joint class="limitedJoints" name="joint_2" range="-2.41 2.41" armature="0.0000015"/>
+        <geom class="visual" mesh="half_arm_1_link" material="white"/>
+        <geom class="collision" mesh="half_arm_1_link_c"/>
+        <body name="half_arm_2_link" gravcomp="1" pos="0 -0.21038 -0.006375" quat="0.707105 -0.707108 0 0">
+          <inertial pos="-4.4e-05 -0.006641 -0.117892" quat="0.706144 0.0213722 -0.0209128 0.707437" mass="1.1636" diaginertia="0.0111633 0.010932 0.00100671"/>
+          <joint class="infiniteJoints" name="joint_3" armature="0.0000015"/>
+          <geom class="visual" mesh="half_arm_2_link" material="white"/>
+          <geom class="collision" mesh="half_arm_2_link_c"/>
+          <body name="forearm_link" gravcomp="1" pos="0 0.006375 -0.21038" quat="0.707105 0.707108 0 0">
+            <inertial pos="-1.8e-05 -0.075478 -0.015006" quat="0.483678 0.515961 -0.515859 0.483455" mass="0.9302" diaginertia="0.00834839 0.008147 0.000598606"/>
+            <joint class="limitedJoints" name="joint_4" range="-2.66 2.66" armature="0.0000015"/>
+            <geom class="visual" mesh="forearm_link" material="white"/>
+            <geom class="collision" mesh="forearm_link_c"/>
+            <body name="spherical_wrist_1_link" gravcomp="1" pos="0 -0.20843 -0.006375" quat="0.707105 -0.707108 0 0">
+              <inertial pos="1e-06 -0.009432 -0.063883" quat="0.703558 0.0707492 -0.0707492 0.703558" mass="0.6781" diaginertia="0.00165901 0.001596 0.000346988"/>
+              <joint class="infiniteJoints" name="joint_5" armature="0.000001928"/>
+              <geom class="visual" mesh="spherical_wrist_1_link" material="white"/>
+              <geom class="collision" mesh="spherical_wrist_1_link_c"/>
+              <body name="spherical_wrist_2_link" gravcomp="1" pos="0 0.00017505 -0.10593" quat="0.707105 0.707108 0 0">
+                <inertial pos="1e-06 -0.045483 -0.00965" quat="0.44426 0.550121 -0.550121 0.44426" mass="0.6781" diaginertia="0.00170087 0.001641 0.00035013"/>
+                <joint class="limitedJoints" name="joint_6" range="-2.23 2.23" armature="0.000001928"/>
+                <geom class="visual" mesh="spherical_wrist_2_link" material="white"/>
+                <geom class="collision" mesh="spherical_wrist_2_link_c"/>
+                <body name="bracelet_link" gravcomp="1" pos="0 -0.10593 -0.00017505" quat="0.707105 -0.707108 0 0">
+                  <camera name="wrist_camera" pos="0.0 -0.056 -0.058" fovy="58" mode="fixed" resolution="640 480" quat="0.0 0.0 0.0 1.0"/>
+                  <site name="wrist_camera_optical_frame" pos="0.0 -0.056 -0.058" quat="0.0 0.0 1.0 0.0"/>
+                  <inertial pos="-9.3e-05 0.000132 -0.022905" quat="-0.0368025 0.719402 -0.0444884 0.69219" mass="0.364" diaginertia="0.00024027 0.000222769 0.000213961"/>
+                  <joint class="infiniteJoints" name="joint_7" armature="0.000001928"/>
+                  <geom class="visual" mesh="bracelet_with_vision_link" material="white"/>
+                  <geom class="collision" mesh="bracelet_with_vision_link_c"/>
                   <body childclass="2f85">
                     <!-- site for end effector's f/t sensor right at the base of the robotiq gripper -->
-                    <site
-                      name="fts"
-                      size="0.01"
-                      pos="0.0 0.0 -0.0615"
-                      quat="0 0.0 1.0 0.0"
-                    />
+                    <site name="fts" size="0.01" pos="0.0 0.0 -0.0615" quat="0 0.0 1.0 0.0"/>
                     <!-- Right-hand side 4-bar linkage -->
-                    <body
-                      name="robotiq_85_base_link"
-                      gravcomp="1"
-                      pos="0 0 -0.0615"
-                      quat="0 -0.707 0.707 0"
-                    >
-                      <inertial
-                        mass="0.777441"
-                        pos="0 -2.70394e-05 0.0354675"
-                        quat="1 -0.00152849 0 0"
-                        diaginertia="0.000260285 0.000225381 0.000152708"
-                      />
-                      <geom
-                        class="visual"
-                        mesh="robotiq_85_base"
-                        material="black"
-                      />
-                      <geom class="collision" mesh="robotiq_85_base" />
-                      <site
-                        name="pinch"
-                        pos="0 0 0.145"
-                        type="sphere"
-                        group="5"
-                        rgba="0.9 0.9 0.9 1"
-                        size="0.005"
-                      />
+                    <body name="robotiq_85_base_link" gravcomp="1" pos="0 0 -0.0615" quat="0 -0.707 0.707 0">
+                      <inertial mass="0.777441" pos="0 -2.70394e-05 0.0354675" quat="1 -0.00152849 0 0" diaginertia="0.000260285 0.000225381 0.000152708"/>
+                      <geom class="visual" mesh="robotiq_85_base" material="black"/>
+                      <geom class="collision" mesh="robotiq_85_base"/>
+                      <site name="pinch" pos="0 0 0.145" type="sphere" group="5" rgba="0.9 0.9 0.9 1" size="0.005"/>
                       <!-- Right-hand side 4-bar linkage -->
-                      <body
-                        name="robotiq_85_right_knuckle_link"
-                        gravcomp="1"
-                        pos="0 0.0306011 0.054904"
-                      >
-                        <inertial
-                          mass="0.00899563"
-                          pos="2.96931e-12 0.0177547 0.00107314"
-                          quat="0.681301 0.732003 0 0"
-                          diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"
-                        />
-                        <joint
-                          name="robotiq_85_right_knuckle_joint"
-                          class="knuckle_link"
-                        />
-                        <geom
-                          class="visual"
-                          mesh="robotiq_85_knuckle_link"
-                          material="gray"
-                        />
-                        <geom
-                          class="collision"
-                          mesh="robotiq_85_knuckle_link"
-                        />
-                        <body
-                          name="robotiq_85_right_finger_link"
-                          gravcomp="1"
-                          pos="0 0.0315 -0.0041"
-                        >
-                          <inertial
-                            mass="0.0140974"
-                            pos="0 0.00301209 0.0232175"
-                            quat="0.705636 -0.0455904 0.0455904 0.705636"
-                            diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"
-                          />
-                          <joint
-                            name="robotiq_85_right_finger_joint"
-                            class="finger_link"
-                          />
-                          <geom
-                            class="visual"
-                            mesh="robotiq_85_finger_link"
-                            material="black"
-                          />
-                          <geom
-                            class="collision"
-                            mesh="robotiq_85_finger_link"
-                          />
+                      <body name="robotiq_85_right_knuckle_link" gravcomp="1" pos="0 0.0306011 0.054904">
+                        <inertial mass="0.00899563" pos="2.96931e-12 0.0177547 0.00107314" quat="0.681301 0.732003 0 0" diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
+                        <joint name="robotiq_85_right_knuckle_joint" class="knuckle_link"/>
+                        <geom class="visual" mesh="robotiq_85_knuckle_link" material="gray"/>
+                        <geom class="collision" mesh="robotiq_85_knuckle_link"/>
+                        <body name="robotiq_85_right_finger_link" gravcomp="1" pos="0 0.0315 -0.0041">
+                          <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636" diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
+                          <joint name="robotiq_85_right_finger_joint" class="finger_link"/>
+                          <geom class="visual" mesh="robotiq_85_finger_link" material="black"/>
+                          <geom class="collision" mesh="robotiq_85_finger_link"/>
                         </body>
                       </body>
-                      <body
-                        name="robotiq_85_right_inner_knuckle_link"
-                        gravcomp="1"
-                        pos="0 0.0132 0.0609"
-                      >
-                        <inertial
-                          mass="0.0221642"
-                          pos="-8.65005e-09 0.0181624 0.0212658"
-                          quat="0.663403 -0.244737 0.244737 0.663403"
-                          diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"
-                        />
-                        <joint
-                          name="robotiq_85_right_inner_knuckle_joint"
-                          class="inner_knuckle_link"
-                        />
-                        <geom
-                          class="visual"
-                          mesh="robotiq_85_inner_knuckle_link"
-                          material="black"
-                        />
-                        <geom
-                          class="collision"
-                          mesh="robotiq_85_inner_knuckle_link"
-                        />
-                        <body
-                          name="robotiq_85_right_finger_tip_link"
-                          gravcomp="1"
-                          pos="0 0.055 0.0375"
-                        >
-                          <inertial
-                            mass="0.0125222"
-                            pos="0 -0.011046 0.0124786"
-                            quat="1 0.1664 0 0"
-                            diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"
-                          />
-                          <joint
-                            name="robotiq_85_right_finger_tip_joint"
-                            class="finger_tip_link"
-                          />
-                          <geom
-                            class="visual"
-                            mesh="robotiq_85_finger_tip_link"
-                            material="black"
-                          />
-                          <geom
-                            class="collision"
-                            mesh="robotiq_85_finger_tip_link"
-                          />
+                      <body name="robotiq_85_right_inner_knuckle_link" gravcomp="1" pos="0 0.0132 0.0609">
+                        <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403" diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+                        <joint name="robotiq_85_right_inner_knuckle_joint" class="inner_knuckle_link"/>
+                        <geom class="visual" mesh="robotiq_85_inner_knuckle_link" material="black"/>
+                        <geom class="collision" mesh="robotiq_85_inner_knuckle_link"/>
+                        <body name="robotiq_85_right_finger_tip_link" gravcomp="1" pos="0 0.055 0.0375">
+                          <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0" diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
+                          <joint name="robotiq_85_right_finger_tip_joint" class="finger_tip_link"/>
+                          <geom class="visual" mesh="robotiq_85_finger_tip_link" material="black"/>
+                          <geom class="collision" mesh="robotiq_85_finger_tip_link"/>
                           <body name="right_pad" pos="0 -0.0189 0.01352">
-                            <geom class="pad_box1" name="right_pad1" />
-                            <geom class="pad_box2" name="right_pad2" />
-                            <inertial
-                              mass="0.0035"
-                              pos="0 -0.0025 0.0185"
-                              quat="0.707107 0 0 0.707107"
-                              diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"
-                            />
-                            <geom class="visual" mesh="robotiq_85_pad" />
+                            <geom class="pad_box1" name="right_pad1"/>
+                            <geom class="pad_box2" name="right_pad2"/>
+                            <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="0.707107 0 0 0.707107" diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
+                            <geom class="visual" mesh="robotiq_85_pad"/>
                             <body name="right_silicone_pad">
-                              <geom
-                                class="visual"
-                                mesh="robotiq_85_silicone_pad"
-                                material="black"
-                              />
+                              <geom class="visual" mesh="robotiq_85_silicone_pad" material="black"/>
                             </body>
-                            <body
-                              name="right_rafti_finger_link"
-                              pos="0 0 0.01"
-                              quat="0 0 0 1"
-                            >
-                              <inertial
-                                mass="0.005"
-                                pos="0 0 0"
-                                diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"
-                              />
-                              <geom
-                                class="visual"
-                                mesh="rafti_finger"
-                                material="black"
-                              />
-                              <geom
-                                class="collision"
-                                mesh="rafti_finger_collision_1"
-                                condim="6"
-                              />
-                              <geom
-                                class="collision"
-                                mesh="rafti_finger_collision_2"
-                                condim="6"
-                              />
-                              <geom
-                                class="collision"
-                                mesh="rafti_finger_collision_3"
-                                condim="6"
-                              />
-                              <geom
-                                class="collision"
-                                mesh="rafti_finger_collision_4"
-                                condim="6"
-                              />
+                            <body name="right_rafti_finger_link" pos="0 0 0.01" quat="0 0 0 1">
+                              <inertial mass="0.005" pos="0 0 0" diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+                              <geom class="visual" mesh="rafti_finger" material="black"/>
+                              <geom class="collision" mesh="rafti_finger_collision_1" condim="6"/>
+                              <geom class="collision" mesh="rafti_finger_collision_2" condim="6"/>
+                              <geom class="collision" mesh="rafti_finger_collision_3" condim="6"/>
+                              <geom class="collision" mesh="rafti_finger_collision_4" condim="6"/>
                             </body>
                           </body>
                         </body>
                       </body>
                       <!-- Left-hand side 4-bar linkage -->
-                      <body
-                        name="robotiq_85_left_knuckle_link"
-                        gravcomp="1"
-                        pos="0 -0.0306011 0.054904"
-                        quat="0 0 0 1"
-                      >
-                        <inertial
-                          mass="0.00899563"
-                          pos="0 0.0177547 0.00107314"
-                          quat="0.681301 0.732003 0 0"
-                          diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"
-                        />
-                        <joint
-                          name="robotiq_85_left_knuckle_joint"
-                          class="knuckle_link"
-                        />
-                        <geom
-                          class="visual"
-                          mesh="robotiq_85_knuckle_link"
-                          material="gray"
-                        />
-                        <geom
-                          class="collision"
-                          mesh="robotiq_85_knuckle_link"
-                        />
-                        <body
-                          name="robotiq_85_left_finger_link"
-                          gravcomp="1"
-                          pos="0 0.0315 -0.0041"
-                        >
-                          <inertial
-                            mass="0.0140974"
-                            pos="0 0.00301209 0.0232175"
-                            quat="0.705636 -0.0455904 0.0455904 0.705636"
-                            diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"
-                          />
-                          <joint
-                            name="robotiq_85_left_finger_joint"
-                            class="finger_link"
-                          />
-                          <geom
-                            class="visual"
-                            mesh="robotiq_85_finger_link"
-                            material="black"
-                          />
-                          <geom
-                            class="collision"
-                            mesh="robotiq_85_finger_link"
-                          />
+                      <body name="robotiq_85_left_knuckle_link" gravcomp="1" pos="0 -0.0306011 0.054904" quat="0 0 0 1">
+                        <inertial mass="0.00899563" pos="0 0.0177547 0.00107314" quat="0.681301 0.732003 0 0" diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
+                        <joint name="robotiq_85_left_knuckle_joint" class="knuckle_link"/>
+                        <geom class="visual" mesh="robotiq_85_knuckle_link" material="gray"/>
+                        <geom class="collision" mesh="robotiq_85_knuckle_link"/>
+                        <body name="robotiq_85_left_finger_link" gravcomp="1" pos="0 0.0315 -0.0041">
+                          <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636" diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
+                          <joint name="robotiq_85_left_finger_joint" class="finger_link"/>
+                          <geom class="visual" mesh="robotiq_85_finger_link" material="black"/>
+                          <geom class="collision" mesh="robotiq_85_finger_link"/>
                         </body>
                       </body>
-                      <body
-                        name="robotiq_85_left_inner_knuckle_link"
-                        gravcomp="1"
-                        pos="0 -0.0132 0.0609"
-                        quat="0 0 0 1"
-                      >
-                        <inertial
-                          mass="0.0221642"
-                          pos="-8.65005e-09 0.0181624 0.0212658"
-                          quat="0.663403 -0.244737 0.244737 0.663403"
-                          diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"
-                        />
-                        <joint
-                          name="robotiq_85_left_inner_knuckle_joint"
-                          class="inner_knuckle_link"
-                        />
-                        <geom
-                          class="visual"
-                          mesh="robotiq_85_inner_knuckle_link"
-                          material="black"
-                        />
-                        <geom
-                          class="collision"
-                          mesh="robotiq_85_inner_knuckle_link"
-                        />
-                        <body
-                          name="robotiq_85_left_finger_tip_link"
-                          gravcomp="1"
-                          pos="0 0.055 0.0375"
-                        >
-                          <inertial
-                            mass="0.0125222"
-                            pos="0 -0.011046 0.0124786"
-                            quat="1 0.1664 0 0"
-                            diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"
-                          />
-                          <joint
-                            name="robotiq_85_left_finger_tip_joint"
-                            class="finger_tip_link"
-                          />
-                          <geom
-                            class="visual"
-                            mesh="robotiq_85_finger_tip_link"
-                            material="black"
-                          />
-                          <geom
-                            class="collision"
-                            mesh="robotiq_85_finger_tip_link"
-                          />
+                      <body name="robotiq_85_left_inner_knuckle_link" gravcomp="1" pos="0 -0.0132 0.0609" quat="0 0 0 1">
+                        <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403" diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+                        <joint name="robotiq_85_left_inner_knuckle_joint" class="inner_knuckle_link"/>
+                        <geom class="visual" mesh="robotiq_85_inner_knuckle_link" material="black"/>
+                        <geom class="collision" mesh="robotiq_85_inner_knuckle_link"/>
+                        <body name="robotiq_85_left_finger_tip_link" gravcomp="1" pos="0 0.055 0.0375">
+                          <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0" diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
+                          <joint name="robotiq_85_left_finger_tip_joint" class="finger_tip_link"/>
+                          <geom class="visual" mesh="robotiq_85_finger_tip_link" material="black"/>
+                          <geom class="collision" mesh="robotiq_85_finger_tip_link"/>
                           <body name="left_pad" pos="0 -0.0189 0.01352">
-                            <geom class="pad_box1" name="left_pad1" />
-                            <geom class="pad_box2" name="left_pad2" />
-                            <inertial
-                              mass="0.0035"
-                              pos="0 -0.0025 0.0185"
-                              quat="1 0 0 1"
-                              diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"
-                            />
-                            <geom class="visual" mesh="robotiq_85_pad" />
+                            <geom class="pad_box1" name="left_pad1"/>
+                            <geom class="pad_box2" name="left_pad2"/>
+                            <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="1 0 0 1" diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
+                            <geom class="visual" mesh="robotiq_85_pad"/>
                             <body name="left_silicone_pad">
-                              <geom
-                                class="visual"
-                                mesh="robotiq_85_silicone_pad"
-                                material="black"
-                              />
+                              <geom class="visual" mesh="robotiq_85_silicone_pad" material="black"/>
                             </body>
-                            <body
-                              name="left_rafti_finger_link"
-                              pos="0 0 0.01"
-                              quat="0 0 0 1"
-                            >
-                              <inertial mass="0.005" pos="0 0 0" />
-                              <geom
-                                class="visual"
-                                mesh="rafti_finger"
-                                material="black"
-                              />
-                              <geom
-                                class="collision"
-                                mesh="rafti_finger_collision_1"
-                                condim="6"
-                              />
-                              <geom
-                                class="collision"
-                                mesh="rafti_finger_collision_2"
-                                condim="6"
-                              />
-                              <geom
-                                class="collision"
-                                mesh="rafti_finger_collision_3"
-                                condim="6"
-                              />
-                              <geom
-                                class="collision"
-                                mesh="rafti_finger_collision_4"
-                                condim="6"
-                              />
+                            <body name="left_rafti_finger_link" pos="0 0 0.01" quat="0 0 0 1">
+                              <inertial mass="0.005" pos="0 0 0"/>
+                              <geom class="visual" mesh="rafti_finger" material="black"/>
+                              <geom class="collision" mesh="rafti_finger_collision_1" condim="6"/>
+                              <geom class="collision" mesh="rafti_finger_collision_2" condim="6"/>
+                              <geom class="collision" mesh="rafti_finger_collision_3" condim="6"/>
+                              <geom class="collision" mesh="rafti_finger_collision_4" condim="6"/>
                             </body>
                           </body>
                         </body>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim/description/mujoco/scene.xml b/src/moveit_pro_kinova_configs/space_satellite_sim/description/mujoco/scene.xml
index 98c7b51e..35c64e4d 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim/description/mujoco/scene.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim/description/mujoco/scene.xml
@@ -1,298 +1,125 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="space satellite scene">
-  <compiler angle="radian" meshdir="assets" />
-  <option integrator="implicitfast" gravity="0 0 0" />
+  <compiler angle="radian" meshdir="assets"/>
+  <option integrator="implicitfast" gravity="0 0 0"/>
 
-  <include file="gen3_7dof_assets.xml" />
+  <include file="gen3_7dof_assets.xml"/>
 
-  <statistic center="0.3 0 0.4" extent="1" />
+  <statistic center="0.3 0 0.4" extent="1"/>
   <default>
-    <geom solref=".004 1" />
+    <geom solref=".004 1"/>
   </default>
 
   <visual>
-    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
-    <rgba haze="0.15 0.25 0.35 1" />
-    <global azimuth="120" elevation="-20" />
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
+    <rgba haze="0.15 0.25 0.35 1"/>
+    <global azimuth="120" elevation="-20"/>
   </visual>
 
   <asset>
-    <texture
-      type="skybox"
-      file="assets/space.png"
-      gridsize="3 4"
-      gridlayout=".U..LFRB.D.."
-    />
-    <texture type="2d" name="ar_marker_0" file="assets/tag36_11_00000.png" />
-    <texture type="2d" name="ar_marker_1" file="assets/tag36_11_00001.png" />
-    <texture type="2d" name="ar_marker_2" file="assets/tag36_11_00002.png" />
-    <material name="ar_marker_0" texture="ar_marker_0" texuniform="false" />
-    <material name="ar_marker_1" texture="ar_marker_1" texuniform="false" />
-    <material name="ar_marker_2" texture="ar_marker_2" texuniform="false" />
-    <material name="landsat_gold" rgba="1.0 0.85 0.6  1" reflectance="0.2" />
-    <material name="metal_rafti" rgba="0.8 0.8 0.8 1" reflectance="0.5" />
-    <material
-      name="servicer_platform_blue"
-      rgba="0.1 0.1 0.7 1"
-      reflectance="0.5"
-    />
-    <material
-      name="servicer_platform_gold"
-      rgba="0.7 0.6 0.4 1"
-      reflectance="0.5"
-    />
-    <material
-      name="servicer_platform_silver"
-      rgba="0.6 0.6 0.6 1"
-      reflectance="0.5"
-    />
-    <mesh name="april_tag" file="april_tag.obj" />
-    <mesh name="landsat" file="landsat/landsat.obj" />
-    <mesh name="landsat_white" file="landsat/landsat_white.obj" />
-    <mesh name="landsat_glass" file="landsat/landsat_glass.obj" />
-    <mesh name="landsat_collision_1" file="landsat/landsat_collision_1.obj" />
-    <mesh name="landsat_collision_2" file="landsat/landsat_collision_2.obj" />
-    <mesh name="landsat_collision_3" file="landsat/landsat_collision_3.obj" />
-    <mesh name="landsat_collision_4" file="landsat/landsat_collision_4.obj" />
-    <mesh name="rafti" file="rafti/rafti.obj" />
-    <mesh name="rafti_collision_1" file="rafti/rafti_collision_1.obj" />
-    <mesh name="rafti_collision_2" file="rafti/rafti_collision_2.obj" />
-    <mesh name="rafti_collision_3" file="rafti/rafti_collision_3.obj" />
-    <mesh name="rafti_collision_4" file="rafti/rafti_collision_4.obj" />
-    <mesh name="rafti_collision_5" file="rafti/rafti_collision_5.obj" />
-    <mesh name="epastar" file="servicer_platform/epastar.obj" />
-    <mesh name="interface" file="servicer_platform/interface.obj" />
-    <mesh name="rpo_suite" file="servicer_platform/rpo_suite.obj" />
-    <mesh name="pole" file="servicer_platform/pole.obj" />
-    <mesh name="solar_panels" file="servicer_platform/solar_panels.obj" />
-    <mesh
-      name="epastar_collision"
-      file="servicer_platform/epastar_collision.obj"
-    />
-    <mesh
-      name="interface_collision"
-      file="servicer_platform/interface_collision.obj"
-    />
-    <mesh
-      name="rpo_suite_collision"
-      file="servicer_platform/rpo_suite_collision.obj"
-    />
-    <mesh name="pole_collision" file="servicer_platform/pole_collision.obj" />
-    <mesh
-      name="solar_panels_collision"
-      file="servicer_platform/solar_panels_collision.obj"
-    />
+    <texture type="skybox" file="assets/space.png" gridsize="3 4" gridlayout=".U..LFRB.D.."/>
+    <texture type="2d" name="ar_marker_0" file="assets/tag36_11_00000.png"/>
+    <texture type="2d" name="ar_marker_1" file="assets/tag36_11_00001.png"/>
+    <texture type="2d" name="ar_marker_2" file="assets/tag36_11_00002.png"/>
+    <material name="ar_marker_0" texture="ar_marker_0" texuniform="false"/>
+    <material name="ar_marker_1" texture="ar_marker_1" texuniform="false"/>
+    <material name="ar_marker_2" texture="ar_marker_2" texuniform="false"/>
+    <material name="landsat_gold" rgba="1.0 0.85 0.6  1" reflectance="0.2"/>
+    <material name="metal_rafti" rgba="0.8 0.8 0.8 1" reflectance="0.5"/>
+    <material name="servicer_platform_blue" rgba="0.1 0.1 0.7 1" reflectance="0.5"/>
+    <material name="servicer_platform_gold" rgba="0.7 0.6 0.4 1" reflectance="0.5"/>
+    <material name="servicer_platform_silver" rgba="0.6 0.6 0.6 1" reflectance="0.5"/>
+    <mesh name="april_tag" file="april_tag.obj"/>
+    <mesh name="landsat" file="landsat/landsat.obj"/>
+    <mesh name="landsat_white" file="landsat/landsat_white.obj"/>
+    <mesh name="landsat_glass" file="landsat/landsat_glass.obj"/>
+    <mesh name="landsat_collision_1" file="landsat/landsat_collision_1.obj"/>
+    <mesh name="landsat_collision_2" file="landsat/landsat_collision_2.obj"/>
+    <mesh name="landsat_collision_3" file="landsat/landsat_collision_3.obj"/>
+    <mesh name="landsat_collision_4" file="landsat/landsat_collision_4.obj"/>
+    <mesh name="rafti" file="rafti/rafti.obj"/>
+    <mesh name="rafti_collision_1" file="rafti/rafti_collision_1.obj"/>
+    <mesh name="rafti_collision_2" file="rafti/rafti_collision_2.obj"/>
+    <mesh name="rafti_collision_3" file="rafti/rafti_collision_3.obj"/>
+    <mesh name="rafti_collision_4" file="rafti/rafti_collision_4.obj"/>
+    <mesh name="rafti_collision_5" file="rafti/rafti_collision_5.obj"/>
+    <mesh name="epastar" file="servicer_platform/epastar.obj"/>
+    <mesh name="interface" file="servicer_platform/interface.obj"/>
+    <mesh name="rpo_suite" file="servicer_platform/rpo_suite.obj"/>
+    <mesh name="pole" file="servicer_platform/pole.obj"/>
+    <mesh name="solar_panels" file="servicer_platform/solar_panels.obj"/>
+    <mesh name="epastar_collision" file="servicer_platform/epastar_collision.obj"/>
+    <mesh name="interface_collision" file="servicer_platform/interface_collision.obj"/>
+    <mesh name="rpo_suite_collision" file="servicer_platform/rpo_suite_collision.obj"/>
+    <mesh name="pole_collision" file="servicer_platform/pole_collision.obj"/>
+    <mesh name="solar_panels_collision" file="servicer_platform/solar_panels_collision.obj"/>
   </asset>
 
   <worldbody>
-    <light pos="1 1 1.5" dir="0 0 -1" directional="true" />
-    <camera
-      name="scene_camera"
-      pos="-0.3 0.3 1.0"
-      fovy="58"
-      mode="fixed"
-      resolution="640 480"
-      euler="0 -1.0472 -1.5708"
-    />
-    <site
-      name="scene_camera_optical_frame"
-      pos="-0.3 0.3 1.0"
-      euler="3.1416 -1.0472 -1.5708"
-      size="0.03"
-    />
+    <light pos="1 1 1.5" dir="0 0 -1" directional="true"/>
+    <camera name="scene_camera" pos="-0.3 0.3 1.0" fovy="58" mode="fixed" resolution="640 480" euler="0 -1.0472 -1.5708"/>
+    <site name="scene_camera_optical_frame" pos="-0.3 0.3 1.0" euler="3.1416 -1.0472 -1.5708" size="0.03"/>
     <body name="landsat">
-      <freejoint name="landsat_joint" />
-      <geom
-        class="visual"
-        type="mesh"
-        material="landsat_gold"
-        mesh="landsat"
-        density="200"
-      />
-      <geom
-        class="visual"
-        type="mesh"
-        rgba="1.0 1.0 1.0 1"
-        mesh="landsat_white"
-        density="200"
-      />
-      <geom
-        class="visual"
-        type="mesh"
-        rgba="0.6 0.6 1.0 1"
-        mesh="landsat_glass"
-        density="200"
-      />
-      <geom
-        class="collision"
-        type="mesh"
-        mesh="landsat_collision_1"
-        density="1"
-      />
-      <geom
-        class="collision"
-        type="mesh"
-        mesh="landsat_collision_2"
-        density="1"
-      />
-      <geom
-        class="collision"
-        type="mesh"
-        mesh="landsat_collision_3"
-        density="1"
-      />
-      <geom
-        class="collision"
-        type="mesh"
-        mesh="landsat_collision_4"
-        density="1"
-      />
+      <freejoint name="landsat_joint"/>
+      <geom class="visual" type="mesh" material="landsat_gold" mesh="landsat" density="200"/>
+      <geom class="visual" type="mesh" rgba="1.0 1.0 1.0 1" mesh="landsat_white" density="200"/>
+      <geom class="visual" type="mesh" rgba="0.6 0.6 1.0 1" mesh="landsat_glass" density="200"/>
+      <geom class="collision" type="mesh" mesh="landsat_collision_1" density="1"/>
+      <geom class="collision" type="mesh" mesh="landsat_collision_2" density="1"/>
+      <geom class="collision" type="mesh" mesh="landsat_collision_3" density="1"/>
+      <geom class="collision" type="mesh" mesh="landsat_collision_4" density="1"/>
       <body name="rafti" pos="-0.565306 -0.3 -1.6" euler="0 -1.5708 0">
-        <geom class="visual" mesh="rafti" material="metal_rafti" />
-        <geom class="collision" mesh="rafti_collision_1" condim="6" />
-        <geom class="collision" mesh="rafti_collision_1" condim="6" />
-        <geom class="collision" mesh="rafti_collision_2" condim="6" />
-        <geom class="collision" mesh="rafti_collision_3" condim="6" />
-        <geom class="collision" mesh="rafti_collision_4" condim="6" />
-        <geom class="collision" mesh="rafti_collision_5" condim="6" />
+        <geom class="visual" mesh="rafti" material="metal_rafti"/>
+        <geom class="collision" mesh="rafti_collision_1" condim="6"/>
+        <geom class="collision" mesh="rafti_collision_1" condim="6"/>
+        <geom class="collision" mesh="rafti_collision_2" condim="6"/>
+        <geom class="collision" mesh="rafti_collision_3" condim="6"/>
+        <geom class="collision" mesh="rafti_collision_4" condim="6"/>
+        <geom class="collision" mesh="rafti_collision_5" condim="6"/>
         <body name="ar_marker_0" pos="0 -.2 .132" quat="1 0 0 0">
-          <geom
-            type="box"
-            size="0.0625 0.0625 0.005"
-            rgba="1 1 1 1"
-            material="ar_marker_0"
-          />
+          <geom type="box" size="0.0625 0.0625 0.005" rgba="1 1 1 1" material="ar_marker_0"/>
         </body>
         <body name="ar_marker_1" pos="0.2 0.2 .132" quat="1 0 0 0">
-          <geom
-            type="box"
-            size="0.0625 0.0625 0.005"
-            rgba="1 1 1 1"
-            material="ar_marker_1"
-          />
+          <geom type="box" size="0.0625 0.0625 0.005" rgba="1 1 1 1" material="ar_marker_1"/>
         </body>
         <body name="ar_marker_2" pos="-0.2 0.2 .132" quat="1 0 0 0">
-          <geom
-            type="box"
-            size="0.0625 0.0625 0.005"
-            rgba="1 1 1 1"
-            material="ar_marker_2"
-          />
+          <geom type="box" size="0.0625 0.0625 0.005" rgba="1 1 1 1" material="ar_marker_2"/>
         </body>
       </body>
     </body>
     <body name="servicer_platform_with_arm">
-      <inertial pos="0 0 0" mass="1000" diaginertia="100 100 100" />
-      <joint name="servicer_platform_x" type="slide" axis="1 0 0" />
-      <joint name="servicer_platform_y" type="slide" axis="0 1 0" />
-      <joint name="servicer_platform_z" type="slide" axis="0 0 1" />
-      <joint name="servicer_platform_roll" type="hinge" axis="1 0 0" />
-      <joint name="servicer_platform_pitch" type="hinge" axis="0 1 0" />
-      <joint name="servicer_platform_yaw" type="hinge" axis="0 0 1" />
+      <inertial pos="0 0 0" mass="1000" diaginertia="100 100 100"/>
+      <joint name="servicer_platform_x" type="slide" axis="1 0 0"/>
+      <joint name="servicer_platform_y" type="slide" axis="0 1 0"/>
+      <joint name="servicer_platform_z" type="slide" axis="0 0 1"/>
+      <joint name="servicer_platform_roll" type="hinge" axis="1 0 0"/>
+      <joint name="servicer_platform_pitch" type="hinge" axis="0 1 0"/>
+      <joint name="servicer_platform_yaw" type="hinge" axis="0 0 1"/>
       <body name="servicer_platform" pos="-0.2874 0.6350 -0.0508" euler="0 0 0">
-        <geom
-          class="visual"
-          type="mesh"
-          mesh="epastar"
-          material="servicer_platform_silver"
-          density="200"
-        />
-        <geom
-          class="visual"
-          type="mesh"
-          mesh="interface"
-          material="servicer_platform_gold"
-          density="200"
-        />
-        <geom
-          class="visual"
-          type="mesh"
-          mesh="rpo_suite"
-          material="servicer_platform_silver"
-          density="200"
-        />
-        <geom
-          class="visual"
-          type="mesh"
-          mesh="pole"
-          material="servicer_platform_silver"
-          density="200"
-        />
-        <geom
-          class="visual"
-          type="mesh"
-          mesh="solar_panels"
-          material="servicer_platform_blue"
-          density="200"
-        />
-        <geom
-          class="collision"
-          type="mesh"
-          mesh="epastar_collision"
-          density="1"
-        />
-        <geom
-          class="collision"
-          type="mesh"
-          mesh="interface_collision"
-          density="1"
-        />
-        <geom
-          class="collision"
-          type="mesh"
-          mesh="rpo_suite_collision"
-          density="1"
-        />
-        <geom class="collision" type="mesh" mesh="pole_collision" density="1" />
-        <geom
-          class="collision"
-          type="mesh"
-          mesh="solar_panels_collision"
-          density="1"
-        />
+        <geom class="visual" type="mesh" mesh="epastar" material="servicer_platform_silver" density="200"/>
+        <geom class="visual" type="mesh" mesh="interface" material="servicer_platform_gold" density="200"/>
+        <geom class="visual" type="mesh" mesh="rpo_suite" material="servicer_platform_silver" density="200"/>
+        <geom class="visual" type="mesh" mesh="pole" material="servicer_platform_silver" density="200"/>
+        <geom class="visual" type="mesh" mesh="solar_panels" material="servicer_platform_blue" density="200"/>
+        <geom class="collision" type="mesh" mesh="epastar_collision" density="1"/>
+        <geom class="collision" type="mesh" mesh="interface_collision" density="1"/>
+        <geom class="collision" type="mesh" mesh="rpo_suite_collision" density="1"/>
+        <geom class="collision" type="mesh" mesh="pole_collision" density="1"/>
+        <geom class="collision" type="mesh" mesh="solar_panels_collision" density="1"/>
       </body>
       <body name="gen3_7dof" pos="0.5 0 0" quat="0.5 0.5 0.5 0.5">
-        <include file="gen3_7dof_body.xml" />
+        <include file="gen3_7dof_body.xml"/>
       </body>
     </body>
   </worldbody>
 
   <actuator>
-    <motor
-      name="servicer_platform_x"
-      joint="servicer_platform_x"
-      ctrllimited="true"
-      ctrlrange="-100 100"
-    />
-    <motor
-      name="servicer_platform_y"
-      joint="servicer_platform_y"
-      ctrllimited="true"
-      ctrlrange="-100 100"
-    />
-    <motor
-      name="servicer_platform_z"
-      joint="servicer_platform_z"
-      ctrllimited="true"
-      ctrlrange="-100 100"
-    />
-    <motor
-      name="servicer_platform_roll"
-      joint="servicer_platform_roll"
-      ctrllimited="true"
-      ctrlrange="-100 100"
-    />
-    <motor
-      name="servicer_platform_pitch"
-      joint="servicer_platform_pitch"
-      ctrllimited="true"
-      ctrlrange="-100 100"
-    />
-    <motor
-      name="servicer_platform_yaw"
-      joint="servicer_platform_yaw"
-      ctrllimited="true"
-      ctrlrange="-100 100"
-    />
+    <motor name="servicer_platform_x" joint="servicer_platform_x" ctrllimited="true" ctrlrange="-100 100"/>
+    <motor name="servicer_platform_y" joint="servicer_platform_y" ctrllimited="true" ctrlrange="-100 100"/>
+    <motor name="servicer_platform_z" joint="servicer_platform_z" ctrllimited="true" ctrlrange="-100 100"/>
+    <motor name="servicer_platform_roll" joint="servicer_platform_roll" ctrllimited="true" ctrlrange="-100 100"/>
+    <motor name="servicer_platform_pitch" joint="servicer_platform_pitch" ctrllimited="true" ctrlrange="-100 100"/>
+    <motor name="servicer_platform_yaw" joint="servicer_platform_yaw" ctrllimited="true" ctrlrange="-100 100"/>
   </actuator>
 
   <keyframe>
@@ -315,37 +142,7 @@
     <!-- ctrl -->
     <!-- joint_1 joint_2 joint_3 joint_4 joint_5 joint_6 joint_7 robotiq_85_right_knuckle_joint -->
     <!-- servicer_platform [x, y, z, roll, pitch, yaw] -->
-    <key
-      name="default"
-      qpos="
-        2.15848 0.515016 1.77308 1 0 0 0
-        0 0 0 0 0 0
-        -1.5708 1.8326 0 -2.3562 0 0.2618 1.5708 0
-        0 0 0 0 0 0 0"
-      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"
-      ctrl="
-        -1.5708 1.8326 0 -2.3562 0 0.2618 1.5708 0
-        0 0 0 0 0 0"
-    />
-    <key
-      name="moving_target_satellite"
-      qpos="
-        2.5 0.5 1.7 1 0 0 0
-        0 0 0 0 0 0
-        -1.5708 1.8326 0 -2.3562 0 0.2618 1.5708 0
-        0 0 0 0 0 0 0"
-      qvel="
-        -0.02 0.02 0 0 0 0.02
-        0 0 0 0 0 0
-        0 0 0 0 0 0 0 0
-        0 0 0 0 0 0 0"
-      ctrl="
-        -1.5708 1.8326 0 -2.3562 0 0.2618 1.5708 0
-        0 0 0 0 0 0"
-    />
+    <key name="default" qpos="         2.15848 0.515016 1.77308 1 0 0 0         0 0 0 0 0 0         -1.5708 1.8326 0 -2.3562 0 0.2618 1.5708 0         0 0 0 0 0 0 0" 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" ctrl="         -1.5708 1.8326 0 -2.3562 0 0.2618 1.5708 0         0 0 0 0 0 0"/>
+    <key name="moving_target_satellite" qpos="         2.5 0.5 1.7 1 0 0 0         0 0 0 0 0 0         -1.5708 1.8326 0 -2.3562 0 0.2618 1.5708 0         0 0 0 0 0 0 0" qvel="         -0.02 0.02 0 0 0 0.02         0 0 0 0 0 0         0 0 0 0 0 0 0 0         0 0 0 0 0 0 0" ctrl="         -1.5708 1.8326 0 -2.3562 0 0.2618 1.5708 0         0 0 0 0 0 0"/>
   </keyframe>
 </mujoco>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim/launch/agent_bridge.launch.xml b/src/moveit_pro_kinova_configs/space_satellite_sim/launch/agent_bridge.launch.xml
index 1179679a..8eeec7d9 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim/launch/agent_bridge.launch.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim/launch/agent_bridge.launch.xml
@@ -1,5 +1,4 @@
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
 </launch>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/close_gripper.xml b/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/close_gripper.xml
index dc790816..31509993 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/close_gripper.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/close_gripper.xml
@@ -1,18 +1,14 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Close Gripper">
   <BehaviorTree ID="Close Gripper" _description="Close the gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.8"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.8"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Close Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/compliant_grasp_landsat.xml b/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/compliant_grasp_landsat.xml
index 9132f7c2..ce762731 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/compliant_grasp_landsat.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/compliant_grasp_landsat.xml
@@ -1,138 +1,46 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Compliant Grasp Landsat">
   <!--//////////-->
-  <BehaviorTree
-    ID="Compliant Grasp Landsat"
-    _description="Grasp RAFTI on the Landsat with the JTAC."
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Compliant Grasp Landsat" _description="Grasp RAFTI on the Landsat with the JTAC." _favorite="true" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="ClearSnapshot" />
-      <SubTree ID="Open Gripper" _collapsed="true" />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="View Tag"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-      />
-      <SubTree ID="Take Snapshot" _collapsed="false" />
-      <SubTree
-        ID="Sample April Tag"
-        _collapsed="false"
-        num_samples="1"
-        apriltag_config="100mm_apriltag_detection_config.yaml"
-        max_distance="0.02"
-        max_rotation="0.2"
-        avg_pose="{tag_pose}"
-        tag_id="0"
-      />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Open Gripper" _collapsed="true"/>
+      <SubTree ID="Move to Waypoint" waypoint_name="View Tag" joint_group_name="manipulator" controller_names="/joint_trajectory_controller" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0"/>
+      <SubTree ID="Take Snapshot" _collapsed="false"/>
+      <SubTree ID="Sample April Tag" _collapsed="false" num_samples="1" apriltag_config="100mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}" tag_id="0"/>
       <Control ID="Fallback">
         <Decorator ID="Timeout" msec="100">
-          <Action
-            ID="PublishStaticFrame"
-            pose="{tag_pose}"
-            child_frame_id="tag_pose"
-            publish_rate="50"
-          />
+          <Action ID="PublishStaticFrame" pose="{tag_pose}" child_frame_id="tag_pose" publish_rate="50"/>
         </Decorator>
-        <Action ID="AlwaysSuccess" />
+        <Action ID="AlwaysSuccess"/>
       </Control>
-      <Action
-        ID="CreateStampedPose"
-        stamped_pose="{rafti_pose}"
-        orientation_xyzw="0.707107;-0.707107;0;0"
-        position_xyz="0.0;-0.2;0.2"
-        reference_frame="tag_pose"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{rafti_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        stamped_pose="{grasp_pose}"
-        orientation_xyzw="0.707107;-0.707107;0;0"
-        position_xyz="0;-0.2;0.06"
-        reference_frame="tag_pose"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{grasp_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <Action
-        ID="TransformPoseFrame"
-        input_pose="{rafti_pose}"
-        output_pose="{rafti_pose_world}"
-        target_frame_id="world"
-      />
+      <Action ID="CreateStampedPose" stamped_pose="{rafti_pose}" orientation_xyzw="0.707107;-0.707107;0;0" position_xyz="0.0;-0.2;0.2" reference_frame="tag_pose"/>
+      <Action ID="AddPoseStampedToVector" input="{rafti_pose}" vector="{pose_stamped_vector}"/>
+      <Action ID="CreateStampedPose" stamped_pose="{grasp_pose}" orientation_xyzw="0.707107;-0.707107;0;0" position_xyz="0;-0.2;0.06" reference_frame="tag_pose"/>
+      <Action ID="AddPoseStampedToVector" input="{grasp_pose}" vector="{pose_stamped_vector}"/>
+      <Action ID="TransformPoseFrame" input_pose="{rafti_pose}" output_pose="{rafti_pose_world}" target_frame_id="world"/>
       <Control ID="Fallback">
         <Decorator ID="Timeout" msec="100">
-          <Action
-            ID="PublishStaticFrame"
-            pose="{rafti_pose_world}"
-            child_frame_id="rafti_pose"
-            publish_rate="50"
-          />
+          <Action ID="PublishStaticFrame" pose="{rafti_pose_world}" child_frame_id="rafti_pose" publish_rate="50"/>
         </Decorator>
-        <Action ID="AlwaysSuccess" />
+        <Action ID="AlwaysSuccess"/>
       </Control>
       <Control ID="Fallback">
-        <Action
-          ID="PlanCartesianPath"
-          blending_radius="0.020000"
-          position_only="false"
-          acceleration_scale_factor="1.000000"
-          debug_solution="{debug_solution}"
-          ik_cartesian_space_density="0.010000"
-          ik_joint_space_density="0.100000"
-          joint_trajectory_msg="{joint_trajectory_msg}"
-          path="{pose_stamped_vector}"
-          planning_group_name="manipulator"
-          tip_link="grasp_link"
-          trajectory_sampling_rate="100"
-          velocity_scale_factor="1.000000"
-        />
-        <Action
-          ID="WaitForUserTrajectoryApproval"
-          solution="{debug_solution}"
-        />
+        <Action ID="PlanCartesianPath" blending_radius="0.020000" position_only="false" acceleration_scale_factor="1.000000" debug_solution="{debug_solution}" ik_cartesian_space_density="0.010000" ik_joint_space_density="0.100000" joint_trajectory_msg="{joint_trajectory_msg}" path="{pose_stamped_vector}" planning_group_name="manipulator" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+        <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
       </Control>
-      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
-      <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true" />
-      <Action
-        ID="ActivateControllers"
-        controller_names="joint_trajectory_admittance_controller"
-      />
-      <Action
-        ID="SetAdmittanceParameters"
-        config_file_name="jtac_parameters.yaml"
-        admittance_parameters_msg="{admittance_parameters_msg}"
-      />
-      <Action
-        ID="ExecuteTrajectoryWithAdmittance"
-        path_position_tolerance="2.0"
-        goal_position_tolerance="0.1000"
-        controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
-        absolute_force_torque_threshold="45;45;45;10;10;10"
-        admittance_parameters_msg="{admittance_parameters_msg}"
-        goal_duration_tolerance="-1.000000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
-      <SubTree ID="Close Gripper" _collapsed="true" />
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
+      <Action ID="ActivateControllers" controller_names="joint_trajectory_admittance_controller"/>
+      <Action ID="SetAdmittanceParameters" config_file_name="jtac_parameters.yaml" admittance_parameters_msg="{admittance_parameters_msg}"/>
+      <Action ID="ExecuteTrajectoryWithAdmittance" path_position_tolerance="2.0" goal_position_tolerance="0.1000" controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory" absolute_force_torque_threshold="45;45;45;10;10;10" admittance_parameters_msg="{admittance_parameters_msg}" goal_duration_tolerance="-1.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
+      <SubTree ID="Close Gripper" _collapsed="true"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Compliant Grasp Landsat">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata subcategory="Application - Advanced Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/open_gripper.xml b/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/open_gripper.xml
index 280fd7e6..be44be79 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/open_gripper.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/open_gripper.xml
@@ -1,19 +1,15 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Open Gripper">
   <!-- ////////// -->
   <BehaviorTree ID="Open Gripper" _description="Open the gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.0"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Open Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/re_zero_force_torque_sensors.xml b/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/re_zero_force_torque_sensors.xml
index 5fbbf52e..43f13d1f 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/re_zero_force_torque_sensors.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/re_zero_force_torque_sensors.xml
@@ -1,26 +1,18 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Re-Zero Force-Torque Sensors">
   <!--//////////-->
-  <BehaviorTree
-    ID="Re-Zero Force-Torque Sensors"
-    _description="Zero the force or torque sensors to mitigate accumulated drift error"
-    _favorite="false"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Re-Zero Force-Torque Sensors" _description="Zero the force or torque sensors to mitigate accumulated drift error" _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <!--Wrap in a ForceSuccess as Sim does not have the zero_ftsensor service-->
       <Decorator ID="ForceSuccess">
-        <Action
-          ID="CallTriggerService"
-          service_name="/io_and_status_controller/zero_fts"
-        />
+        <Action ID="CallTriggerService" service_name="/io_and_status_controller/zero_fts"/>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Re-Zero Force-Torque Sensors">
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/sample_april_tag.xml b/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/sample_april_tag.xml
index 0f26bbba..456b2d07 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/sample_april_tag.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/sample_april_tag.xml
@@ -1,63 +1,27 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Sample April Tag">
   <!--//////////-->
-  <BehaviorTree
-    ID="Sample April Tag"
-    _description="Samples the pose of an AprilTag and returns the average measurement."
-    _favorite="false"
-  >
+  <BehaviorTree ID="Sample April Tag" _description="Samples the pose of an AprilTag and returns the average measurement." _favorite="false">
     <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_camera/camera_info"
-          message_out="{camera_info}"
-        />
+        <Action ID="LoadObjectiveParameters" config_file_name="{apriltag_config}" parameters="{parameters}"/>
+        <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" message_out="{camera_info}"/>
         <Decorator ID="RetryUntilSuccessful" num_attempts="5">
           <Control ID="Sequence">
-            <Action
-              ID="GetImage"
-              topic_name="/wrist_camera/color"
-              message_out="{image}"
-            />
-            <Action
-              ID="DetectAprilTags"
-              image="{image}"
-              camera_info="{camera_info}"
-              parameters="{parameters}"
-              detections="{detections}"
-            />
-            <Action
-              ID="GetDetectionPose"
-              detections="{detections}"
-              target_label=""
-              detection_pose="{detection_pose}"
-              target_id="{tag_id}"
-            />
+            <Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}"/>
+            <Action ID="DetectAprilTags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/>
+            <Action ID="GetDetectionPose" detections="{detections}" target_label="" detection_pose="{detection_pose}" target_id="{tag_id}"/>
           </Control>
         </Decorator>
-        <Action
-          ID="AveragePoseStamped"
-          pose_sample="{detection_pose}"
-          avg_pose="{avg_pose}"
-          max_distance="{max_distance}"
-          max_rotation="{max_rotation}"
-          num_samples="{num_samples}"
-          run_continuously="false"
-        />
+        <Action ID="AveragePoseStamped" pose_sample="{detection_pose}" avg_pose="{avg_pose}" max_distance="{max_distance}" max_rotation="{max_rotation}" num_samples="{num_samples}" run_continuously="false"/>
       </Control>
     </Decorator>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Sample April Tag">
-      <input_port name="tag_id" default="-1" />
+      <input_port name="tag_id" default="-1"/>
       <MetadataFields>
-        <Metadata subcategory="Perception - Image Processing" />
+        <Metadata subcategory="Perception - Image Processing"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/take_snapshot.xml b/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/take_snapshot.xml
index 57a512d8..6861b36f 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/take_snapshot.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim/objectives/take_snapshot.xml
@@ -1,22 +1,18 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Take Snapshot">
   <!--//////////-->
-  <BehaviorTree
-    ID="Take Snapshot"
-    _description="Take a pointcloud snapshot of the scene with a depth camera"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Take Snapshot" _description="Take a pointcloud snapshot of the scene with a depth camera" _favorite="true">
     <Control ID="Sequence">
       <!-- Clear out old snapshot data -->
-      <Action ID="ClearSnapshot" />
-      <Action ID="GetPointCloud" topic_name="/wrist_camera/points" />
-      <Action ID="SendPointCloudToUI" />
+      <Action ID="ClearSnapshot"/>
+      <Action ID="GetPointCloud" topic_name="/wrist_camera/points"/>
+      <Action ID="SendPointCloudToUI"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Take Snapshot">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_kinova_configs/space_satellite_sim/package.xml b/src/moveit_pro_kinova_configs/space_satellite_sim/package.xml
index 5d43ec28..9ccefc21 100644
--- a/src/moveit_pro_kinova_configs/space_satellite_sim/package.xml
+++ b/src/moveit_pro_kinova_configs/space_satellite_sim/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>space_satellite_sim</name>
   <version>0.1.0</version>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/description/scene.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/description/scene.xml
index 68621d62..2732dd94 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/description/scene.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/description/scene.xml
@@ -1,43 +1,27 @@
+<?xml version='1.0' encoding='utf-8'?>
 <!-- UR5e inspired from https://github.com/google-deepmind/mujoco_menagerie/tree/main/universal_robots_ur5e -->
 <mujoco model="ur5e scene">
-  <include file="ur5e/ur5e_globals.xml" />
-  <include file="lab_desk/desk_globals.xml" />
-  <include file="robotiq_2f85/robotiq2f85_globals.xml" />
-  <include file="ridgeback/ridgeback_globals.xml" />
+  <include file="ur5e/ur5e_globals.xml"/>
+  <include file="lab_desk/desk_globals.xml"/>
+  <include file="robotiq_2f85/robotiq2f85_globals.xml"/>
+  <include file="ridgeback/ridgeback_globals.xml"/>
 
-  <compiler angle="radian" autolimits="true" />
+  <compiler angle="radian" autolimits="true"/>
 
-  <option integrator="implicitfast" />
-  <statistic center="0.3 0 0.4" extent="0.8" />
+  <option integrator="implicitfast"/>
+  <statistic center="0.3 0 0.4" extent="0.8"/>
   <asset>
-    <mesh name="airfoil" file="assets/airfoil.stl" />
+    <mesh name="airfoil" file="assets/airfoil.stl"/>
   </asset>
   <worldbody>
-    <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
-    <geom
-      name="floor"
-      pos="0 0 -0.04"
-      size="0 0 0.05"
-      type="plane"
-      material="groundplane"
-    />
-    <site
-      name="scene_camera_optical_frame"
-      pos="0.0 -2.0 3.5"
-      euler="3.64 0.0 0.0"
-    />
-    <camera
-      name="scene_camera"
-      pos="0.5 -4.0 3.5"
-      fovy="58"
-      mode="fixed"
-      resolution="640 480"
-      euler="0.8 -0.4 -0.2"
-    />
+    <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
+    <geom name="floor" pos="0 0 -0.04" size="0 0 0.05" type="plane" material="groundplane"/>
+    <site name="scene_camera_optical_frame" pos="0.0 -2.0 3.5" euler="3.64 0.0 0.0"/>
+    <camera name="scene_camera" pos="0.5 -4.0 3.5" fovy="58" mode="fixed" resolution="640 480" euler="0.8 -0.4 -0.2"/>
     <body name="airfoil" pos="4.5 0 0.5" euler="0.0 0.0 1.5707">
-      <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="airfoil" />
+      <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="airfoil"/>
     </body>
-    <include file="ridgeback/ridgeback.xml" />
+    <include file="ridgeback/ridgeback.xml"/>
   </worldbody>
   <keyframe>
     <!-- qpos -->
@@ -46,15 +30,6 @@
     <!-- right_knuckle, right_finger, right_inner_knuckle, right_finger_tip, left_knuckle, left_finger, left_inner_knuckle, left_finger_tip -->
     <!-- ctrl -->
     <!-- linear_x_joint, linear_y_joint, rotational_yaw_joint, shoulder_pan, shoulder_lift, elbow, wrist_1, wrist_2, wrist_3, robotiq_85_left_knuckle_joint -->
-    <key
-      qpos="
-      0 0 0 0 0
-      0 0 0 0 -1.534 0.8197 -2.2686 -1.57076 0
-      0 0 0 0 0 0 0 0
-    "
-      ctrl="
-      0 0 0 0 -1.534 0.8197 -2.2686 -1.57076 0 0
-    "
-    />
+    <key qpos="       0 0 0 0 0       0 0 0 0 -1.534 0.8197 -2.2686 -1.57076 0       0 0 0 0 0 0 0 0     " ctrl="       0 0 0 0 -1.534 0.8197 -2.2686 -1.57076 0 0     "/>
   </keyframe>
 </mujoco>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/launch/agent_bridge.launch.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/launch/agent_bridge.launch.xml
index b2aa7543..8eeec7d9 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/launch/agent_bridge.launch.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/launch/agent_bridge.launch.xml
@@ -1,6 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
 </launch>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml
index 9dc774db..da2d9d49 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml
@@ -1,28 +1,16 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="00 Solution - Move to Initial Pose"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="00 Solution - Move to Initial Pose">
   <!--//////////-->
-  <BehaviorTree
-    ID="00 Solution - Move to Initial Pose"
-    _description="Move to a pre-defined waypoint"
-    _favorite="false"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="00 Solution - Move to Initial Pose" _description="Move to a pre-defined waypoint" _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Look at Airfoil"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
+      <SubTree ID="Move to Waypoint" waypoint_name="Look at Airfoil" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="00 Solution - Move to Initial Pose">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/01_solution_-_create_a_pose.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/01_solution_-_create_a_pose.xml
index f267967a..d618bb9d 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/01_solution_-_create_a_pose.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/01_solution_-_create_a_pose.xml
@@ -1,24 +1,16 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="01 Solution - Create a Pose">
   <!--//////////-->
-  <BehaviorTree
-    ID="01 Solution - Create a Pose"
-    _description="Creates a pose relative to a frame"
-    _favorite="false"
-  >
+  <BehaviorTree ID="01 Solution - Create a Pose" _description="Creates a pose relative to a frame" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0"
-        reference_frame="grasp_link"
-      />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0" reference_frame="grasp_link"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="01 Solution - Create a Pose">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/02_solution_-_visualize_a_pose.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/02_solution_-_visualize_a_pose.xml
index 436ce4e8..36603dd2 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/02_solution_-_visualize_a_pose.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/02_solution_-_visualize_a_pose.xml
@@ -1,25 +1,17 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="02 Solution - Visualize a Pose">
   <!--//////////-->
-  <BehaviorTree
-    ID="02 Solution - Visualize a Pose"
-    _description="Creates a pose and visualizes it"
-    _favorite="false"
-  >
+  <BehaviorTree ID="02 Solution - Visualize a Pose" _description="Creates a pose and visualizes it" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="VisualizePose" marker_size="0.200000" />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="VisualizePose" marker_size="0.200000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="02 Solution - Visualize a Pose">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/03_solution_-_pose_2m_above_the_robot.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/03_solution_-_pose_2m_above_the_robot.xml
index 1df2d2cc..5e484194 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/03_solution_-_pose_2m_above_the_robot.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/03_solution_-_pose_2m_above_the_robot.xml
@@ -1,28 +1,17 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="03 Solution - Pose 2m above the robot"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="03 Solution - Pose 2m above the robot">
   <!--//////////-->
-  <BehaviorTree
-    ID="03 Solution - Pose 2m above the robot"
-    _description="Creates a visualizes a pose 2m right above the robot 'base_link'"
-    _favorite="false"
-  >
+  <BehaviorTree ID="03 Solution - Pose 2m above the robot" _description="Creates a visualizes a pose 2m right above the robot 'base_link'" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;2"
-        reference_frame="base_link"
-      />
-      <Action ID="VisualizePose" marker_size="0.200000" />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;2" reference_frame="base_link"/>
+      <Action ID="VisualizePose" marker_size="0.200000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="03 Solution - Pose 2m above the robot">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/04_solution_-_move_to_a_pose.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/04_solution_-_move_to_a_pose.xml
index 66cac31a..012861a9 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/04_solution_-_move_to_a_pose.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/04_solution_-_move_to_a_pose.xml
@@ -1,32 +1,18 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="04 Solution - Move to a Pose">
   <!--//////////-->
-  <BehaviorTree
-    ID="04 Solution - Move to a Pose"
-    _description="Creates a pose relative to the tool, and moves to it"
-    _favorite="true"
-  >
+  <BehaviorTree ID="04 Solution - Move to a Pose" _description="Creates a pose relative to the tool, and moves to it" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;1"
-        reference_frame="grasp_link"
-      />
-      <Action ID="VisualizePose" marker_size="0.200000" />
-      <SubTree
-        ID="Move to a StampedPose"
-        _collapsed="true"
-        target_pose="{stamped_pose}"
-        controller_names="/joint_trajectory_controller"
-      />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;1" reference_frame="grasp_link"/>
+      <Action ID="VisualizePose" marker_size="0.200000"/>
+      <SubTree ID="Move to a StampedPose" _collapsed="true" target_pose="{stamped_pose}" controller_names="/joint_trajectory_controller"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="04 Solution - Move to a Pose">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/05_solution_-_define_a_path.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/05_solution_-_define_a_path.xml
index 7e033561..73a5e8b7 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/05_solution_-_define_a_path.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/05_solution_-_define_a_path.xml
@@ -1,32 +1,19 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="05 Solution - Define a path">
   <!--//////////-->
-  <BehaviorTree
-    ID="05 Solution - Define a path"
-    _description="Creates a simple path with two waypoints"
-    _favorite="false"
-  >
+  <BehaviorTree ID="05 Solution - Define a path" _description="Creates a simple path with two waypoints" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0.2"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0.2" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="05 Solution - Define a path">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/06_solution_-_visualize_a_path.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/06_solution_-_visualize_a_path.xml
index e1f7f592..6130c500 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/06_solution_-_visualize_a_path.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/06_solution_-_visualize_a_path.xml
@@ -1,33 +1,20 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="06 Solution - Visualize a path">
   <!--//////////-->
-  <BehaviorTree
-    ID="06 Solution - Visualize a path"
-    _description="How to visualize a path"
-    _favorite="false"
-  >
+  <BehaviorTree ID="06 Solution - Visualize a path" _description="How to visualize a path" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0.2"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action ID="VisualizePath" path="{pose_stamped_vector}" />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0.2" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePath" path="{pose_stamped_vector}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="06 Solution - Visualize a path">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/07_solution_-_plan_motion_for_a_path.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/07_solution_-_plan_motion_for_a_path.xml
index f52f0998..79eed402 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/07_solution_-_plan_motion_for_a_path.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/07_solution_-_plan_motion_for_a_path.xml
@@ -1,30 +1,19 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="07 Solution - Plan motion for a path"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="07 Solution - Plan motion for a path">
   <!--//////////-->
-  <BehaviorTree
-    ID="07 Solution - Plan motion for a path"
-    _description="Given a path, plans the motion for the robot joints"
-    _favorite="false"
-  >
+  <BehaviorTree ID="07 Solution - Plan motion for a path" _description="Given a path, plans the motion for the robot joints" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0.2"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action ID="VisualizePose" />
-      <Action ID="PlanCartesianPath" />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0.2" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="PlanCartesianPath"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="07 Solution - Plan motion for a path">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/08_solution_-_execute_path_plan.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/08_solution_-_execute_path_plan.xml
index 4190a29d..d03568f5 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/08_solution_-_execute_path_plan.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/08_solution_-_execute_path_plan.xml
@@ -1,28 +1,20 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="08 Solution - Execute path plan">
   <!--//////////-->
-  <BehaviorTree
-    ID="08 Solution - Execute path plan"
-    _description="Plans joint-space motion to track a path, and executes it"
-    _favorite="false"
-  >
+  <BehaviorTree ID="08 Solution - Execute path plan" _description="Plans joint-space motion to track a path, and executes it" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0.2"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action ID="VisualizePose" />
-      <Action ID="PlanCartesianPath" />
-      <Action ID="ExecuteFollowJointTrajectory" />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0.2" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="PlanCartesianPath"/>
+      <Action ID="ExecuteFollowJointTrajectory"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="08 Solution - Execute path plan">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/09_solution_-_an_infeasible_path.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/09_solution_-_an_infeasible_path.xml
index 2daac747..aa31d32a 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/09_solution_-_an_infeasible_path.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/09_solution_-_an_infeasible_path.xml
@@ -1,31 +1,19 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="09 Solution - An infeasible path">
   <!--//////////-->
-  <BehaviorTree
-    ID="09 Solution - An infeasible path"
-    _description="An example of a path that is not feasible"
-    _favorite="false"
-  >
+  <BehaviorTree ID="09 Solution - An infeasible path" _description="An example of a path that is not feasible" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="1;0;-0.5"
-        reference_frame="world"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action ID="VisualizePose" />
-      <Action
-        ID="PlanCartesianPath"
-        ik_cartesian_space_density="0.01000"
-        ik_joint_space_density="0.01000"
-      />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="1;0;-0.5" reference_frame="world"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="PlanCartesianPath" ik_cartesian_space_density="0.01000" ik_joint_space_density="0.01000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="09 Solution - An infeasible path">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/10_solution_-_visualize_path_error.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/10_solution_-_visualize_path_error.xml
index 45465e71..45a5d4e4 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/10_solution_-_visualize_path_error.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/10_solution_-_visualize_path_error.xml
@@ -1,40 +1,22 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="10 Solution - Visualize path error"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="10 Solution - Visualize path error">
   <!--//////////-->
-  <BehaviorTree
-    ID="10 Solution - Visualize path error"
-    _description="How to visualize what's going on with an infeasible path"
-    _favorite="false"
-  >
+  <BehaviorTree ID="10 Solution - Visualize path error" _description="How to visualize what's going on with an infeasible path" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="1;0;-1"
-        reference_frame="world"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action ID="VisualizePose" />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="1;0;-1" reference_frame="world"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
       <Control ID="Fallback">
-        <Action
-          ID="PlanCartesianPath"
-          ik_cartesian_space_density="0.01000"
-          ik_joint_space_density="0.01000"
-        />
-        <Action
-          ID="WaitForUserTrajectoryApproval"
-          solution="{debug_solution}"
-        />
+        <Action ID="PlanCartesianPath" ik_cartesian_space_density="0.01000" ik_joint_space_density="0.01000"/>
+        <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="10 Solution - Visualize path error">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/11_solution_-_draw_a_square.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/11_solution_-_draw_a_square.xml
index 94b25389..948e6716 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/11_solution_-_draw_a_square.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/11_solution_-_draw_a_square.xml
@@ -1,54 +1,26 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="11 Solution - Draw a square">
   <!--//////////-->
-  <BehaviorTree
-    ID="11 Solution - Draw a square"
-    _description="Hands-on exercise: draw a square"
-    _favorite="true"
-  >
+  <BehaviorTree ID="11 Solution - Draw a square" _description="Hands-on exercise: draw a square" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0.4;0;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0.4;-0.4;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;-0.4;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action ID="VisualizePath" path="{pose_stamped_vector}" />
-      <Action
-        ID="PlanCartesianPath"
-        ik_joint_space_density="0.01000"
-        ik_cartesian_space_density="0.01000"
-      />
-      <Action ID="ExecuteFollowJointTrajectory" />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0.4;0;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0.4;-0.4;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;-0.4;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePath" path="{pose_stamped_vector}"/>
+      <Action ID="PlanCartesianPath" ik_joint_space_density="0.01000" ik_cartesian_space_density="0.01000"/>
+      <Action ID="ExecuteFollowJointTrajectory"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="11 Solution - Draw a square">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/12_solution_-_constrain_orientation.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/12_solution_-_constrain_orientation.xml
index a842a476..e57ebafc 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/12_solution_-_constrain_orientation.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/12_solution_-_constrain_orientation.xml
@@ -1,57 +1,26 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="12 Solution - Constrain orientation"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="12 Solution - Constrain orientation">
   <!--//////////-->
-  <BehaviorTree
-    ID="12 Solution - Constrain orientation"
-    _description="How to constrain end-effector orientation along paths"
-    _favorite="false"
-  >
+  <BehaviorTree ID="12 Solution - Constrain orientation" _description="How to constrain end-effector orientation along paths" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0.4;0;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0.4;-0.4;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;-0.4;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action ID="VisualizePath" path="{pose_stamped_vector}" />
-      <Action
-        ID="PlanCartesianPath"
-        ik_joint_space_density="0.01000"
-        ik_cartesian_space_density="0.01000"
-        position_only="false"
-      />
-      <Action ID="ExecuteFollowJointTrajectory" />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0.4;0;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0.4;-0.4;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;-0.4;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePath" path="{pose_stamped_vector}"/>
+      <Action ID="PlanCartesianPath" ik_joint_space_density="0.01000" ik_cartesian_space_density="0.01000" position_only="false"/>
+      <Action ID="ExecuteFollowJointTrajectory"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="14 Solution - Constrain Orientation">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/13_solution_-_load_a_path_from_file.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/13_solution_-_load_a_path_from_file.xml
index dbcac425..7916516e 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/13_solution_-_load_a_path_from_file.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/13_solution_-_load_a_path_from_file.xml
@@ -1,27 +1,16 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="13 Solution - Load a path from file"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="13 Solution - Load a path from file">
   <!--//////////-->
-  <BehaviorTree
-    ID="13 Solution - Load a path from file"
-    _description="Loads a complex path from a file, and visualizes it"
-    _favorite="false"
-  >
+  <BehaviorTree ID="13 Solution - Load a path from file" _description="Loads a complex path from a file, and visualizes it" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="LoadPoseStampedVectorFromYaml" file_path="path.yaml" />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;1;0"
-        position_xyz="0;0;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="VisualizePose" />
+      <Action ID="LoadPoseStampedVectorFromYaml" file_path="path.yaml"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;1;0" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="VisualizePose"/>
       <Control ID="Parallel" success_count="1" failure_count="1">
-        <Action ID="PublishStaticFrame" child_frame_id="local" />
+        <Action ID="PublishStaticFrame" child_frame_id="local"/>
         <Control ID="Sequence">
-          <Action ID="WaitForDuration" delay_duration="0.5" />
-          <Action ID="VisualizePath" path="{pose_stamped_msgs}" />
+          <Action ID="WaitForDuration" delay_duration="0.5"/>
+          <Action ID="VisualizePath" path="{pose_stamped_msgs}"/>
         </Control>
       </Control>
     </Control>
@@ -29,8 +18,8 @@
   <TreeNodesModel>
     <SubTree ID="13 Solution - Load a path from file">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/14_solution_-_load_from_file_execute.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/14_solution_-_load_from_file_execute.xml
index e15f9a5e..bebf62bb 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/14_solution_-_load_from_file_execute.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/14_solution_-_load_from_file_execute.xml
@@ -1,39 +1,19 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="14 Solution - Load from file, execute"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="14 Solution - Load from file, execute">
   <!--//////////-->
-  <BehaviorTree
-    ID="14 Solution - Load from file, execute"
-    _description="Loads a path from a file, plans motion and executes it"
-    _favorite="true"
-  >
+  <BehaviorTree ID="14 Solution - Load from file, execute" _description="Loads a path from a file, plans motion and executes it" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Close Gripper" _collapsed="true" />
-      <Action ID="LoadPoseStampedVectorFromYaml" file_path="path.yaml" />
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;1;0"
-        position_xyz="0;0;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="VisualizePose" />
+      <SubTree ID="Close Gripper" _collapsed="true"/>
+      <Action ID="LoadPoseStampedVectorFromYaml" file_path="path.yaml"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;1;0" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="VisualizePose"/>
       <Control ID="Parallel" success_count="1" failure_count="1">
-        <Action ID="PublishStaticFrame" child_frame_id="local" />
+        <Action ID="PublishStaticFrame" child_frame_id="local"/>
         <Control ID="Sequence">
-          <Action ID="WaitForDuration" delay_duration="0.5" />
-          <Action ID="VisualizePath" path="{pose_stamped_msgs}" />
-          <Action
-            ID="PlanCartesianPath"
-            path="{pose_stamped_msgs}"
-            ik_joint_space_density="0.1000"
-            ik_cartesian_space_density="0.01000"
-            blending_radius="0.001"
-            position_only="false"
-            tip_offset="0;0;0.04;0;0;0"
-          />
-          <Action ID="ExecuteFollowJointTrajectory" />
+          <Action ID="WaitForDuration" delay_duration="0.5"/>
+          <Action ID="VisualizePath" path="{pose_stamped_msgs}"/>
+          <Action ID="PlanCartesianPath" path="{pose_stamped_msgs}" ik_joint_space_density="0.1000" ik_cartesian_space_density="0.01000" blending_radius="0.001" position_only="false" tip_offset="0;0;0.04;0;0;0"/>
+          <Action ID="ExecuteFollowJointTrajectory"/>
         </Control>
       </Control>
     </Control>
@@ -41,8 +21,8 @@
   <TreeNodesModel>
     <SubTree ID="14 Solution - Load from file, execute">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/15_solution_-_a_path_that_collides.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/15_solution_-_a_path_that_collides.xml
index ca742734..7d22c35b 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/15_solution_-_a_path_that_collides.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/15_solution_-_a_path_that_collides.xml
@@ -1,36 +1,20 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="15 Solution - A path that collides"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="15 Solution - A path that collides">
   <!--//////////-->
-  <BehaviorTree
-    ID="15 Solution - A path that collides"
-    _description="An example of a path kinematically feasible, but colliding"
-    _favorite="false"
-  >
+  <BehaviorTree ID="15 Solution - A path that collides" _description="An example of a path kinematically feasible, but colliding" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;-1;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action ID="VisualizePose" />
-      <Action
-        ID="PlanCartesianPath"
-        ik_cartesian_space_density="0.01000"
-        ik_joint_space_density="0.01000"
-        position_only="false"
-      />
-      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;-1;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="PlanCartesianPath" ik_cartesian_space_density="0.01000" ik_joint_space_density="0.01000" position_only="false"/>
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="15 Solution - A path that collides">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/16_solution_-_checking_for_collisions.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/16_solution_-_checking_for_collisions.xml
index 73627786..69b3b5c8 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/16_solution_-_checking_for_collisions.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/16_solution_-_checking_for_collisions.xml
@@ -1,46 +1,24 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="16 Solution - Checking for collisions"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="16 Solution - Checking for collisions">
   <!--//////////-->
-  <BehaviorTree
-    ID="16 Solution - Checking for collisions"
-    _description="How to check if a plan will collide"
-    _favorite="false"
-  >
+  <BehaviorTree ID="16 Solution - Checking for collisions" _description="How to check if a plan will collide" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;-1;0"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action ID="VisualizePose" />
-      <Action
-        ID="PlanCartesianPath"
-        ik_cartesian_space_density="0.01000"
-        ik_joint_space_density="0.01000"
-        position_only="false"
-      />
-      <Action
-        ID="GetCurrentPlanningScene"
-        planning_scene_msg="{planning_scene_msg}"
-      />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;-1;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="PlanCartesianPath" ik_cartesian_space_density="0.01000" ik_joint_space_density="0.01000" position_only="false"/>
+      <Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene_msg}"/>
       <Control ID="Fallback">
-        <Action ID="ValidateTrajectory" debug_solution="{debug_solution}" />
-        <Action
-          ID="WaitForUserTrajectoryApproval"
-          solution="{debug_solution}"
-        />
+        <Action ID="ValidateTrajectory" debug_solution="{debug_solution}"/>
+        <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="16 Solution - Checking for collisions">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/17_solution_-_execute_the_feasible_path.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/17_solution_-_execute_the_feasible_path.xml
index f73ac8e1..b44cb367 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/17_solution_-_execute_the_feasible_path.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/17_solution_-_execute_the_feasible_path.xml
@@ -1,50 +1,25 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="17 Solution - Execute the feasible path"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="17 Solution - Execute the feasible path">
   <!--//////////-->
-  <BehaviorTree
-    ID="17 Solution - Execute the feasible path"
-    _description=""
-    _favorite="false"
-  >
+  <BehaviorTree ID="17 Solution - Execute the feasible path" _description="" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;5"
-        reference_frame="grasp_link"
-      />
-      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" />
-      <Action ID="VisualizePose" />
-      <Action
-        ID="PlanCartesianPath"
-        ik_cartesian_space_density="0.01000"
-        ik_joint_space_density="0.01000"
-        position_only="false"
-      />
-      <Action
-        ID="GetCurrentPlanningScene"
-        planning_scene_msg="{planning_scene_msg}"
-      />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;5" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="PlanCartesianPath" ik_cartesian_space_density="0.01000" ik_joint_space_density="0.01000" position_only="false"/>
+      <Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene_msg}"/>
       <Control ID="Fallback">
-        <Action ID="ValidateTrajectory" debug_solution="{debug_solution}" />
-        <Action
-          ID="WaitForUserTrajectoryApproval"
-          solution="{debug_solution}"
-        />
+        <Action ID="ValidateTrajectory" debug_solution="{debug_solution}"/>
+        <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
       </Control>
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <Action ID="ExecuteFollowJointTrajectory" joint_trajectory_msg="{joint_trajectory_msg}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="17 Solution - Execute the feasible path">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/18_solution_-_define_a_coverage_path.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/18_solution_-_define_a_coverage_path.xml
index 1e3ce7a8..ce2b77d3 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/18_solution_-_define_a_coverage_path.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/18_solution_-_define_a_coverage_path.xml
@@ -1,37 +1,19 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="18 Solution - Define a coverage path"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="18 Solution - Define a coverage path">
   <!--//////////-->
-  <BehaviorTree
-    ID="18 Solution - Define a coverage path"
-    _description="Automatically creates a path to cover a rectangular area"
-    _favorite="false"
-  >
+  <BehaviorTree ID="18 Solution - Define a coverage path" _description="Automatically creates a path to cover a rectangular area" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;-0.5;0.2"
-        reference_frame="grasp_link"
-      />
-      <Action ID="VisualizePose" />
-      <Action
-        ID="GenerateCoveragePath"
-        stride_distance="0.1"
-        width="0.6"
-        height="0.6"
-        bottom_right_corner="{stamped_pose}"
-        vector_of_poses="{pose_stamped_vector}"
-      />
-      <Action ID="VisualizePath" path="{pose_stamped_vector}" />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;-0.5;0.2" reference_frame="grasp_link"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="GenerateCoveragePath" stride_distance="0.1" width="0.6" height="0.6" bottom_right_corner="{stamped_pose}" vector_of_poses="{pose_stamped_vector}"/>
+      <Action ID="VisualizePath" path="{pose_stamped_vector}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="18 Solution - Define a coverage path">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/19_solution_-_execute_coverage_path.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/19_solution_-_execute_coverage_path.xml
index e180e6b7..408b3258 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/19_solution_-_execute_coverage_path.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/19_solution_-_execute_coverage_path.xml
@@ -1,45 +1,21 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="19 Solution - Execute coverage path"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="19 Solution - Execute coverage path">
   <!--//////////-->
-  <BehaviorTree
-    ID="19 Solution - Execute coverage path"
-    _description="Executes a coverage path"
-    _favorite="false"
-  >
+  <BehaviorTree ID="19 Solution - Execute coverage path" _description="Executes a coverage path" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;-0.5;0.2"
-        reference_frame="grasp_link"
-      />
-      <Action ID="VisualizePose" />
-      <Action
-        ID="GenerateCoveragePath"
-        stride_distance="0.1"
-        width="0.6"
-        height="0.6"
-        bottom_right_corner="{stamped_pose}"
-        vector_of_poses="{pose_stamped_vector}"
-      />
-      <Action ID="VisualizePath" path="{pose_stamped_vector}" />
-      <Action
-        ID="PlanCartesianPath"
-        ik_joint_space_density="0.01000"
-        ik_cartesian_space_density="0.01000"
-        position_only="false"
-        path="{pose_stamped_vector}"
-      />
-      <Action ID="ExecuteFollowJointTrajectory" />
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;-0.5;0.2" reference_frame="grasp_link"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="GenerateCoveragePath" stride_distance="0.1" width="0.6" height="0.6" bottom_right_corner="{stamped_pose}" vector_of_poses="{pose_stamped_vector}"/>
+      <Action ID="VisualizePath" path="{pose_stamped_vector}"/>
+      <Action ID="PlanCartesianPath" ik_joint_space_density="0.01000" ik_cartesian_space_density="0.01000" position_only="false" path="{pose_stamped_vector}"/>
+      <Action ID="ExecuteFollowJointTrajectory"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="19 Solution - Execute coverage path">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/20_solution_-_look_at_the_airfoil.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/20_solution_-_look_at_the_airfoil.xml
index 6901884b..8fd68ccb 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/20_solution_-_look_at_the_airfoil.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/20_solution_-_look_at_the_airfoil.xml
@@ -1,25 +1,16 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="20 Solution - Look at the airfoil">
   <!--//////////-->
-  <BehaviorTree
-    ID="20 Solution - Look at the airfoil"
-    _description="Move to a pre-defined waypoint that looks towards the airfoil"
-    _favorite="false"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="20 Solution - Look at the airfoil" _description="Move to a pre-defined waypoint that looks towards the airfoil" _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Look at Airfoil"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
+      <SubTree ID="Move to Waypoint" waypoint_name="Look at Airfoil" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="20 Solution - Look at the airfoil">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/21_solution_-_find_the_airfoil.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/21_solution_-_find_the_airfoil.xml
index ee48f5f1..5bc35740 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/21_solution_-_find_the_airfoil.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/21_solution_-_find_the_airfoil.xml
@@ -1,35 +1,18 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="21 Solution - Find the airfoil">
   <!--//////////-->
-  <BehaviorTree
-    ID="21 Solution - Find the airfoil"
-    _description="Estimate the pose of an airfoil using ICP"
-    _favorite="false"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="21 Solution - Find the airfoil" _description="Estimate the pose of an airfoil using ICP" _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="ClearSnapshot" />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Look at Airfoil"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
-      <SubTree
-        ID="Estimate Object Pose"
-        object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl"
-        guess_position="4.5;0.0;0.5"
-        guess_orientation="0.0;0.0;0.7071;0.7071"
-        icp_max_correspondence_distance="0.5"
-        model_to_real_pose="{model_to_real_pose}"
-        _collapsed="true"
-      />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Move to Waypoint" waypoint_name="Look at Airfoil" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <SubTree ID="Estimate Object Pose" object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl" guess_position="4.5;0.0;0.5" guess_orientation="0.0;0.0;0.7071;0.7071" icp_max_correspondence_distance="0.5" model_to_real_pose="{model_to_real_pose}" _collapsed="true"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="21 Solution - Find the airfoil">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/22_solution_-_visualize_airfoil_frame.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/22_solution_-_visualize_airfoil_frame.xml
index 8e7b893f..c7c20443 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/22_solution_-_visualize_airfoil_frame.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/22_solution_-_visualize_airfoil_frame.xml
@@ -1,39 +1,19 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="22 Solution - Visualize airfoil frame"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="22 Solution - Visualize airfoil frame">
   <!--//////////-->
-  <BehaviorTree
-    ID="22 Solution - Visualize airfoil frame"
-    _description="Estimate the pose of an airfoil and visualize its origin frame"
-    _favorite="false"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="22 Solution - Visualize airfoil frame" _description="Estimate the pose of an airfoil and visualize its origin frame" _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="ClearSnapshot" />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Look at Airfoil"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
-      <SubTree
-        ID="Estimate Object Pose"
-        object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl"
-        guess_position="4.5;0.0;0.5"
-        guess_orientation="0.0;0.0;0.7071;0.7071"
-        icp_max_correspondence_distance="0.5"
-        model_to_real_pose="{model_to_real_pose}"
-        _collapsed="true"
-      />
-      <Action ID="VisualizePose" pose="{model_to_real_pose}" />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Move to Waypoint" waypoint_name="Look at Airfoil" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <SubTree ID="Estimate Object Pose" object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl" guess_position="4.5;0.0;0.5" guess_orientation="0.0;0.0;0.7071;0.7071" icp_max_correspondence_distance="0.5" model_to_real_pose="{model_to_real_pose}" _collapsed="true"/>
+      <Action ID="VisualizePose" pose="{model_to_real_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="22 Solution - Visualize airfoil frame">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/23_solution_-_airfoil_coverage_origin.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/23_solution_-_airfoil_coverage_origin.xml
index 7008ed4a..50319001 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/23_solution_-_airfoil_coverage_origin.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/23_solution_-_airfoil_coverage_origin.xml
@@ -1,45 +1,20 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="23 Solution - Airfoil Coverage Origin"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="23 Solution - Airfoil Coverage Origin">
   <!--//////////-->
-  <BehaviorTree
-    ID="23 Solution - Airfoil Coverage Origin"
-    _description="Estimate the pose of an airfoil and set the origin to define a coverage area"
-    _favorite="false"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="23 Solution - Airfoil Coverage Origin" _description="Estimate the pose of an airfoil and set the origin to define a coverage area" _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="ClearSnapshot" />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Look at Airfoil"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
-      <SubTree
-        ID="Estimate Object Pose"
-        object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl"
-        guess_position="4.5;0.0;0.5"
-        guess_orientation="0.0;0.0;0.7071;0.7071"
-        icp_max_correspondence_distance="0.5"
-        model_to_real_pose="{model_to_real_pose}"
-        _collapsed="true"
-      />
-      <Action
-        ID="TransformPose"
-        input_pose="{model_to_real_pose}"
-        translation_xyz="0.0; 0.2; -0.25"
-        quaternion_xyzw="0.7071068; 0; 0; 0.7071068 "
-      />
-      <Action ID="VisualizePose" pose="{output_pose}" marker_size="0.150000" />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Move to Waypoint" waypoint_name="Look at Airfoil" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <SubTree ID="Estimate Object Pose" object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl" guess_position="4.5;0.0;0.5" guess_orientation="0.0;0.0;0.7071;0.7071" icp_max_correspondence_distance="0.5" model_to_real_pose="{model_to_real_pose}" _collapsed="true"/>
+      <Action ID="TransformPose" input_pose="{model_to_real_pose}" translation_xyz="0.0; 0.2; -0.25" quaternion_xyzw="0.7071068; 0; 0; 0.7071068 "/>
+      <Action ID="VisualizePose" pose="{output_pose}" marker_size="0.150000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="23 Solution - Airfoil Coverage Origin">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/24_solution_-_airfoil_coverage_path.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/24_solution_-_airfoil_coverage_path.xml
index e4fb1758..611d35d8 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/24_solution_-_airfoil_coverage_path.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/24_solution_-_airfoil_coverage_path.xml
@@ -1,53 +1,22 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="24 Solution - Airfoil Coverage Path"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="24 Solution - Airfoil Coverage Path">
   <!--//////////-->
-  <BehaviorTree
-    ID="24 Solution - Airfoil Coverage Path"
-    _description="Estimate the pose of an airfoil and define a coverage path"
-    _favorite="false"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="24 Solution - Airfoil Coverage Path" _description="Estimate the pose of an airfoil and define a coverage path" _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="ClearSnapshot" />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Look at Airfoil"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
-      <SubTree
-        ID="Estimate Object Pose"
-        object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl"
-        guess_position="4.5;0.0;0.5"
-        guess_orientation="0.0;0.0;0.7071;0.7071"
-        icp_max_correspondence_distance="0.5"
-        model_to_real_pose="{model_to_real_pose}"
-        _collapsed="true"
-      />
-      <Action
-        ID="TransformPose"
-        input_pose="{model_to_real_pose}"
-        translation_xyz="0.0; 0.2; -0.25"
-        quaternion_xyzw="0.7071068; 0; 0; 0.7071068 "
-      />
-      <Action ID="VisualizePose" pose="{output_pose}" marker_size="0.150000" />
-      <Action
-        ID="GenerateCoveragePath"
-        stride_distance="0.2"
-        width="1"
-        height="0.8"
-        bottom_right_corner="{output_pose}"
-      />
-      <Action ID="VisualizePath" path="{pose_stamped_vector}" />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Move to Waypoint" waypoint_name="Look at Airfoil" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <SubTree ID="Estimate Object Pose" object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl" guess_position="4.5;0.0;0.5" guess_orientation="0.0;0.0;0.7071;0.7071" icp_max_correspondence_distance="0.5" model_to_real_pose="{model_to_real_pose}" _collapsed="true"/>
+      <Action ID="TransformPose" input_pose="{model_to_real_pose}" translation_xyz="0.0; 0.2; -0.25" quaternion_xyzw="0.7071068; 0; 0; 0.7071068 "/>
+      <Action ID="VisualizePose" pose="{output_pose}" marker_size="0.150000"/>
+      <Action ID="GenerateCoveragePath" stride_distance="0.2" width="1" height="0.8" bottom_right_corner="{output_pose}"/>
+      <Action ID="VisualizePath" path="{pose_stamped_vector}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="24 Solution - Airfoil Coverage Path">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/25_solution_-_execute_airfoil_coverage.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/25_solution_-_execute_airfoil_coverage.xml
index 30ef3c97..87e7c8ad 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/25_solution_-_execute_airfoil_coverage.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/25_solution_-_execute_airfoil_coverage.xml
@@ -1,61 +1,25 @@
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="25 Solution - Execute Airfoil Coverage"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="25 Solution - Execute Airfoil Coverage">
   <!--//////////-->
-  <BehaviorTree
-    ID="25 Solution - Execute Airfoil Coverage"
-    _description="Estimate the pose of an airfoil, define a coverage path and execute it"
-    _favorite="false"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="25 Solution - Execute Airfoil Coverage" _description="Estimate the pose of an airfoil, define a coverage path and execute it" _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="ClearSnapshot" />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Look at Airfoil"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
-      <SubTree
-        ID="Estimate Object Pose"
-        object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl"
-        guess_position="4.5;0.0;0.5"
-        guess_orientation="0.0;0.0;0.7071;0.7071"
-        icp_max_correspondence_distance="0.5"
-        model_to_real_pose="{model_to_real_pose}"
-        _collapsed="true"
-      />
-      <Action
-        ID="TransformPose"
-        input_pose="{model_to_real_pose}"
-        translation_xyz="0.0; 0.2; -0.25"
-        quaternion_xyzw="0.7071068; 0; 0; 0.7071068 "
-      />
-      <Action ID="VisualizePose" pose="{output_pose}" marker_size="0.150000" />
-      <Action
-        ID="GenerateCoveragePath"
-        stride_distance="0.2"
-        width="1"
-        height="0.8"
-        bottom_right_corner="{output_pose}"
-      />
-      <Action ID="VisualizePath" path="{pose_stamped_vector}" />
-      <Action
-        ID="PlanCartesianPath"
-        ik_joint_space_density="0.01000"
-        ik_cartesian_space_density="0.01000"
-        position_only="true"
-      />
-      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
-      <Action ID="ExecuteFollowJointTrajectory" />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Move to Waypoint" waypoint_name="Look at Airfoil" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <SubTree ID="Estimate Object Pose" object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl" guess_position="4.5;0.0;0.5" guess_orientation="0.0;0.0;0.7071;0.7071" icp_max_correspondence_distance="0.5" model_to_real_pose="{model_to_real_pose}" _collapsed="true"/>
+      <Action ID="TransformPose" input_pose="{model_to_real_pose}" translation_xyz="0.0; 0.2; -0.25" quaternion_xyzw="0.7071068; 0; 0; 0.7071068 "/>
+      <Action ID="VisualizePose" pose="{output_pose}" marker_size="0.150000"/>
+      <Action ID="GenerateCoveragePath" stride_distance="0.2" width="1" height="0.8" bottom_right_corner="{output_pose}"/>
+      <Action ID="VisualizePath" path="{pose_stamped_vector}"/>
+      <Action ID="PlanCartesianPath" ik_joint_space_density="0.01000" ik_cartesian_space_density="0.01000" position_only="true"/>
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      <Action ID="ExecuteFollowJointTrajectory"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="25 Solution - Execute Airfoil Coverage">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/26_solution_-_better_coverage.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/26_solution_-_better_coverage.xml
index 97f3c894..d9da8d9d 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/26_solution_-_better_coverage.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/26_solution_-_better_coverage.xml
@@ -1,69 +1,25 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="26 Solution - Better Coverage">
   <!--//////////-->
-  <BehaviorTree
-    ID="26 Solution - Better Coverage"
-    _description="Try to cover as much airfoil surface as possible"
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="26 Solution - Better Coverage" _description="Try to cover as much airfoil surface as possible" _favorite="true" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="ClearSnapshot" />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Look at Airfoil"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
-      <SubTree
-        ID="Estimate Object Pose"
-        object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl"
-        guess_position="4.5;0.0;0.5"
-        guess_orientation="0.0;0.0;0.7071068; 0.7071068"
-        icp_max_correspondence_distance="0.5"
-        model_to_real_pose="{model_to_real_pose}"
-        _collapsed="true"
-      />
-      <Action
-        ID="TransformPose"
-        quaternion_xyzw="0.5;-0.5;0.5;0.5"
-        translation_xyz="2.5;0.2;-0.2"
-        input_pose="{model_to_real_pose}"
-        output_pose="{model_to_real_pose}"
-      />
-      <Action
-        ID="GenerateCoveragePath"
-        stride_distance="0.4"
-        bottom_right_corner="{model_to_real_pose}"
-        vector_of_poses="{pose_stamped_vector}"
-        width="0.6"
-        height="5"
-      />
-      <Action ID="VisualizePath" path="{pose_stamped_vector}" />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{model_to_real_pose}"
-        vector="{path_to_first_waypoint}"
-      />
-      <Action
-        ID="PlanCartesianPath"
-        tip_offset="0;0;0;-0.5;0.0;1.5707"
-        planning_group_name="manipulator"
-        path="{pose_stamped_vector}"
-        position_only="false"
-        trajectory_sampling_rate="50"
-        ik_joint_space_density="0.05000"
-        ik_cartesian_space_density="0.02000"
-      />
-      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
-      <Action ID="ExecuteFollowJointTrajectory" />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Move to Waypoint" waypoint_name="Look at Airfoil" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <SubTree ID="Estimate Object Pose" object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl" guess_position="4.5;0.0;0.5" guess_orientation="0.0;0.0;0.7071068; 0.7071068" icp_max_correspondence_distance="0.5" model_to_real_pose="{model_to_real_pose}" _collapsed="true"/>
+      <Action ID="TransformPose" quaternion_xyzw="0.5;-0.5;0.5;0.5" translation_xyz="2.5;0.2;-0.2" input_pose="{model_to_real_pose}" output_pose="{model_to_real_pose}"/>
+      <Action ID="GenerateCoveragePath" stride_distance="0.4" bottom_right_corner="{model_to_real_pose}" vector_of_poses="{pose_stamped_vector}" width="0.6" height="5"/>
+      <Action ID="VisualizePath" path="{pose_stamped_vector}"/>
+      <Action ID="AddPoseStampedToVector" input="{model_to_real_pose}" vector="{path_to_first_waypoint}"/>
+      <Action ID="PlanCartesianPath" tip_offset="0;0;0;-0.5;0.0;1.5707" planning_group_name="manipulator" path="{pose_stamped_vector}" position_only="false" trajectory_sampling_rate="50" ik_joint_space_density="0.05000" ik_cartesian_space_density="0.02000"/>
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      <Action ID="ExecuteFollowJointTrajectory"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="26 Solution - Better Coverage">
       <MetadataFields>
-        <Metadata subcategory="Application - Training Solution" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Training Solution"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/draw_picknik.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/draw_picknik.xml
index e28bc878..94123664 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/draw_picknik.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/draw_picknik.xml
@@ -1,73 +1,19 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Draw Picknik">
   <!--//////////-->
-  <BehaviorTree
-    ID="Draw Picknik"
-    _description=""
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Draw Picknik" _description="" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="LoadPoseStampedVectorFromYaml"
-        output="{pose_stamped_msgs}"
-        file_path="path.yaml"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;1;0"
-        position_xyz="0;0;0"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="pose"
-        marker_size="0.100000"
-        pose="{stamped_pose}"
-      />
+      <Action ID="LoadPoseStampedVectorFromYaml" output="{pose_stamped_msgs}" file_path="path.yaml"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;1;0" position_xyz="0;0;0"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{stamped_pose}"/>
       <Control ID="Parallel" failure_count="1" success_count="2">
-        <Action
-          ID="PublishStaticFrame"
-          child_frame_id="local"
-          pose="{stamped_pose}"
-          publish_rate="50"
-        />
+        <Action ID="PublishStaticFrame" child_frame_id="local" pose="{stamped_pose}" publish_rate="50"/>
         <Control ID="Sequence">
-          <Action ID="WaitForDuration" delay_duration="1" />
-          <Action
-            ID="PlanCartesianPath"
-            acceleration_scale_factor="1.000000"
-            blending_radius="0.001"
-            debug_solution="{debug_solution}"
-            ik_cartesian_space_density="0.010000"
-            ik_joint_space_density="0.100000"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-            path="{pose_stamped_msgs}"
-            planning_group_name="manipulator"
-            position_only="true"
-            tip_link="grasp_link"
-            trajectory_sampling_rate="100"
-            velocity_scale_factor="1.000000"
-          />
-          <Action
-            ID="VisualizePath"
-            marker_lifetime="0.000000"
-            path="{pose_stamped_msgs}"
-          />
-          <Action
-            ID="WaitForUserTrajectoryApproval"
-            solution="{debug_solution}"
-          />
-          <Action
-            ID="ExecuteFollowJointTrajectory"
-            execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-            goal_duration_tolerance="-1.000000"
-            goal_position_tolerance="0.000000"
-            goal_time_tolerance="0.000000"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-          />
+          <Action ID="WaitForDuration" delay_duration="1"/>
+          <Action ID="PlanCartesianPath" acceleration_scale_factor="1.000000" blending_radius="0.001" debug_solution="{debug_solution}" ik_cartesian_space_density="0.010000" ik_joint_space_density="0.100000" joint_trajectory_msg="{joint_trajectory_msg}" path="{pose_stamped_msgs}" planning_group_name="manipulator" position_only="true" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+          <Action ID="VisualizePath" marker_lifetime="0.000000" path="{pose_stamped_msgs}"/>
+          <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+          <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
         </Control>
       </Control>
     </Control>
@@ -75,8 +21,8 @@
   <TreeNodesModel>
     <SubTree ID="Draw Picknik">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples " />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Basic Examples "/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/draw_square.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/draw_square.xml
index 72438dba..70b6c093 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/draw_square.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/draw_square.xml
@@ -1,122 +1,30 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Draw Square">
   <!--//////////-->
-  <BehaviorTree
-    ID="Draw Square"
-    _description=""
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Draw Square" _description="" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        waypoint_name="Look at Airfoil"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0.4"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{stamped_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;-0.4;0.4"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{stamped_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0.4;-0.4;0.4"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{stamped_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0.4;0;0.4"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{stamped_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0.4"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{stamped_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <Action
-        ID="VisualizePath"
-        marker_lifetime="0.000000"
-        path="{pose_stamped_vector}"
-      />
-      <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="{pose_stamped_vector}"
-        planning_group_name="manipulator"
-        position_only="false"
-        tip_link="grasp_link"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="1.000000"
-      />
-      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-        goal_duration_tolerance="-1.000000"
-        goal_position_tolerance="0.000000"
-        goal_time_tolerance="0.000000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0" waypoint_name="Look at Airfoil"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;0;0.4"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;-0.4;0.4"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0.4;-0.4;0.4"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0.4;0;0.4"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;0;0.4"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+      <Action ID="VisualizePath" marker_lifetime="0.000000" path="{pose_stamped_vector}"/>
+      <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="{pose_stamped_vector}" planning_group_name="manipulator" position_only="false" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Draw Square">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Basic Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/estimate_object_pose.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/estimate_object_pose.xml
index 1414ad50..a65228dc 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/estimate_object_pose.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/estimate_object_pose.xml
@@ -1,79 +1,23 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Estimate Object Pose">
   <!--//////////-->
-  <BehaviorTree
-    ID="Estimate Object Pose"
-    _description="Estimate an object pose from a reference mesh file using ICP on live point cloud data."
-    _favorite="false"
-    _hardcoded="false"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Estimate Object Pose" _description="Estimate an object pose from a reference mesh file using ICP on live point cloud data." _favorite="false" _hardcoded="false" _subtreeOnly="true">
     <Control ID="Sequence">
-      <Action
-        ID="LoadPointCloudFromFile"
-        file_path="{object_model_file_path}"
-        frame_id="world"
-        num_sampled_points="10000"
-        random_seed="1234"
-        point_cloud="{model_point_cloud}"
-        scale="1.0"
-        color="255;150;0"
-      />
-      <Action
-        ID="GetSyncedImageAndPointCloud"
-        point_cloud_topic_name="/wrist_camera/points"
-        rgb_image_topic_name="/wrist_camera/color"
-        rgb_camera_info_topic_name="/wrist_camera/camera_info"
-        point_cloud="{point_cloud}"
-        rgb_image="{rgb_image}"
-        rgb_camera_info="{rgb_camera_info}"
-      />
-      <Action
-        ID="TransformPointCloudFrame"
-        input_cloud="{point_cloud}"
-        target_frame="world"
-        output_cloud="{point_cloud}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="world"
-        position_xyz="{guess_position}"
-        orientation_xyzw="{guess_orientation}"
-        stamped_pose="{object_stamped_pose_estimate}"
-      />
-      <Action
-        ID="TransformPointCloud"
-        input_cloud="{model_point_cloud}"
-        transform_pose="{object_stamped_pose_estimate}"
-        output_cloud="{model_point_cloud}"
-      />
-      <Action
-        ID="RegisterPointClouds"
-        base_point_cloud="{model_point_cloud}"
-        target_point_cloud="{point_cloud}"
-        max_iterations="30"
-        max_correspondence_distance="{icp_max_correspondence_distance}"
-        target_pose_in_base_frame="{model_to_real_pose}"
-      />
-      <Action
-        ID="TransformPointCloud"
-        input_cloud="{model_point_cloud}"
-        transform_pose="{model_to_real_pose}"
-        output_cloud="{aligned_model_cloud}"
-      />
-      <Action ID="SendPointCloudToUI" point_cloud="{aligned_model_cloud}" />
-      <Action
-        ID="TransformPoseWithPose"
-        input_pose="{object_stamped_pose_estimate}"
-        transform_pose="{model_to_real_pose}"
-        output_pose="{model_to_real_pose}"
-      />
+      <Action ID="LoadPointCloudFromFile" file_path="{object_model_file_path}" frame_id="world" num_sampled_points="10000" random_seed="1234" point_cloud="{model_point_cloud}" scale="1.0" color="255;150;0"/>
+      <Action ID="GetSyncedImageAndPointCloud" point_cloud_topic_name="/wrist_camera/points" rgb_image_topic_name="/wrist_camera/color" rgb_camera_info_topic_name="/wrist_camera/camera_info" point_cloud="{point_cloud}" rgb_image="{rgb_image}" rgb_camera_info="{rgb_camera_info}"/>
+      <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" target_frame="world" output_cloud="{point_cloud}"/>
+      <Action ID="CreateStampedPose" reference_frame="world" position_xyz="{guess_position}" orientation_xyzw="{guess_orientation}" stamped_pose="{object_stamped_pose_estimate}"/>
+      <Action ID="TransformPointCloud" input_cloud="{model_point_cloud}" transform_pose="{object_stamped_pose_estimate}" output_cloud="{model_point_cloud}"/>
+      <Action ID="RegisterPointClouds" base_point_cloud="{model_point_cloud}" target_point_cloud="{point_cloud}" max_iterations="30" max_correspondence_distance="{icp_max_correspondence_distance}" target_pose_in_base_frame="{model_to_real_pose}"/>
+      <Action ID="TransformPointCloud" input_cloud="{model_point_cloud}" transform_pose="{model_to_real_pose}" output_cloud="{aligned_model_cloud}"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{aligned_model_cloud}"/>
+      <Action ID="TransformPoseWithPose" input_pose="{object_stamped_pose_estimate}" transform_pose="{model_to_real_pose}" output_pose="{model_to_real_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Estimate Object Pose">
       <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
+        <Metadata subcategory="Perception - 3D Point Cloud"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/follow_path.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/follow_path.xml
index 2d6226cf..46b2d960 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/follow_path.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/follow_path.xml
@@ -1,99 +1,27 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Follow Path">
   <!--//////////-->
-  <BehaviorTree
-    ID="Follow Path"
-    _description=""
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Follow Path" _description="" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        waypoint_name="Look at Airfoil"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;0.2"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="pose"
-        marker_size="0.100000"
-        pose="{stamped_pose}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{stamped_pose}"
-        vector="{pose_stamped_vector}"
-      />
-      <Action
-        ID="VisualizePath"
-        marker_lifetime="0.000000"
-        path="{pose_stamped_vector}"
-      />
-      <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="{pose_stamped_vector}"
-        planning_group_name="manipulator"
-        position_only="true"
-        tip_link="grasp_link"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="1.000000"
-      />
-      <Action
-        ID="GetCurrentPlanningScene"
-        planning_scene_msg="{planning_scene}"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0" waypoint_name="Look at Airfoil"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;0;0.2"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{stamped_pose}"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+      <Action ID="VisualizePath" marker_lifetime="0.000000" path="{pose_stamped_vector}"/>
+      <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="{pose_stamped_vector}" planning_group_name="manipulator" position_only="true" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+      <Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene}"/>
       <Control ID="Fallback">
-        <Action
-          ID="ValidateTrajectory"
-          cartesian_space_step="0.020000"
-          debug_solution="{debug_solution}"
-          joint_space_step="0.100000"
-          joint_trajectory_msg="{joint_trajectory_msg}"
-          planning_group_name="manipulator"
-          planning_scene_msg="{planning_scene}"
-        />
-        <Action
-          ID="WaitForUserTrajectoryApproval"
-          solution="{debug_solution}"
-        />
+        <Action ID="ValidateTrajectory" cartesian_space_step="0.020000" debug_solution="{debug_solution}" joint_space_step="0.100000" joint_trajectory_msg="{joint_trajectory_msg}" planning_group_name="manipulator" planning_scene_msg="{planning_scene}"/>
+        <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
       </Control>
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-        goal_duration_tolerance="-1.000000"
-        goal_position_tolerance="0.000000"
-        goal_time_tolerance="0.000000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Follow Path">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Motion - Mobile Base" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Motion - Mobile Base"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/generate_coverage_path.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/generate_coverage_path.xml
index f639a326..03924b8c 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/generate_coverage_path.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/generate_coverage_path.xml
@@ -1,64 +1,21 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Generate coverage path">
   <!--//////////-->
-  <BehaviorTree
-    ID="Generate coverage path"
-    _description=""
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Generate coverage path" _description="" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;1;0"
-        position_xyz="0;0;0.1"
-      />
-      <Action
-        ID="GenerateCoveragePath"
-        vector_of_poses="{pose_stamped_vector}"
-        bottom_right_corner="{stamped_pose}"
-        height="0.6"
-        stride_distance="0.1"
-        width="0.6"
-      />
-      <Action
-        ID="VisualizePath"
-        marker_lifetime="0.000000"
-        path="{pose_stamped_vector}"
-      />
-      <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="{pose_stamped_vector}"
-        planning_group_name="manipulator"
-        position_only="false"
-        tip_link="grasp_link"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="1.000000"
-      />
-      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-        goal_duration_tolerance="-1.000000"
-        goal_position_tolerance="0.000000"
-        goal_time_tolerance="0.000000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;1;0" position_xyz="0;0;0.1"/>
+      <Action ID="GenerateCoveragePath" vector_of_poses="{pose_stamped_vector}" bottom_right_corner="{stamped_pose}" height="0.6" stride_distance="0.1" width="0.6"/>
+      <Action ID="VisualizePath" marker_lifetime="0.000000" path="{pose_stamped_vector}"/>
+      <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="{pose_stamped_vector}" planning_group_name="manipulator" position_only="false" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Generate coverage path">
       <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata runnable="true"/>
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_2m_pose.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_2m_pose.xml
index 01782f2a..0438c5c2 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_2m_pose.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_2m_pose.xml
@@ -1,53 +1,19 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move 2m pose">
   <!--//////////-->
-  <BehaviorTree
-    ID="Move 2m pose"
-    _description=""
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Move 2m pose" _description="" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        waypoint_name="Look at Airfoil"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        stamped_pose="{stamped_pose}"
-        orientation_xyzw="0;0;0;1"
-        position_xyz="0;0;2"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="pose"
-        marker_size="0.100000"
-        pose="{stamped_pose}"
-      />
-      <SubTree
-        ID="Move to a StampedPose"
-        _collapsed="true"
-        controller_names="/joint_trajectory_controller"
-        target_pose="{stamped_pose}"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0" waypoint_name="Look at Airfoil"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" stamped_pose="{stamped_pose}" orientation_xyzw="0;0;0;1" position_xyz="0;0;2"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{stamped_pose}"/>
+      <SubTree ID="Move to a StampedPose" _collapsed="true" controller_names="/joint_trajectory_controller" target_pose="{stamped_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move 2m pose">
       <MetadataFields>
-        <Metadata subcategory="Motion - Mobile Base" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Motion - Mobile Base"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml
index e2acbc4c..2895bdbe 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml
@@ -1,63 +1,28 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to a StampedPose">
-  <BehaviorTree
-    ID="Move to a StampedPose"
-    _description="Uses inverse kinematics to move the robot tip to a given pose"
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Move to a StampedPose" _description="Uses inverse kinematics to move the robot tip to a given pose" _subtreeOnly="true">
     <Control ID="Fallback" name="root">
       <Control ID="Sequence">
-        <Action
-          ID="InitializeMTCTask"
-          task_id="move_to_pose"
-          controller_names="{controller_names}"
-          task="{move_to_pose_task}"
-        />
-        <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
-        <Action
-          ID="SetupMTCPlanToPose"
-          ik_frame="grasp_link"
-          planning_group_name="manipulator"
-          target_pose="{target_pose}"
-          task="{move_to_pose_task}"
-        />
-        <Action
-          ID="PlanMTCTask"
-          solution="{move_to_pose_solution}"
-          task="{move_to_pose_task}"
-        />
-        <SubTree
-          ID="Wait for Trajectory Approval if User Available"
-          solution="{move_to_pose_solution}"
-        />
-        <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
-        <Action
-          ID="PublishEmpty"
-          topic="/studio_ui/motion_ended"
-          queue_size="1"
-          use_best_effort="false"
-        />
+        <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="{controller_names}" task="{move_to_pose_task}"/>
+        <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
+        <Action ID="SetupMTCPlanToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}"/>
+        <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+        <SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
+        <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}"/>
+        <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
       </Control>
       <Control ID="Sequence">
-        <Action
-          ID="PublishEmpty"
-          topic="/studio_ui/motion_ended"
-          queue_size="1"
-          use_best_effort="false"
-        />
-        <Action ID="AlwaysFailure" />
+        <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+        <Action ID="AlwaysFailure"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to a StampedPose">
-      <input_port
-        name="controller_names"
-        default="/joint_trajectory_controller"
-      />
-      <input_port name="target_pose" default="{stamped_pose}" />
+      <input_port name="controller_names" default="/joint_trajectory_controller"/>
+      <input_port name="target_pose" default="{stamped_pose}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/scan_and_plan.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/scan_and_plan.xml
index 7cc17588..976dba86 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/scan_and_plan.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/scan_and_plan.xml
@@ -1,93 +1,24 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Scan and Plan">
   <!--//////////-->
-  <BehaviorTree
-    ID="Scan and Plan"
-    _description=""
-    _favorite="false"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Scan and Plan" _description="" _favorite="false" _subtreeOnly="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/joint_trajectory_controller"
-        joint_group_name="manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-        waypoint_name="Look at Airfoil"
-      />
-      <SubTree
-        ID="Estimate Object Pose"
-        _collapsed="true"
-        guess_orientation="0;0;0.707;0.707"
-        guess_position="4.5;0;0.5"
-        icp_max_correspondence_distance="0.5"
-        object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl"
-        stamped_pose="{stamped_pose}"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="pose"
-        marker_size="0.100000"
-        pose="{stamped_pose}"
-      />
-      <Action
-        ID="TransformPose"
-        input_pose="{stamped_pose}"
-        output_pose="{output_pose}"
-        quaternion_xyzw="0.5;-0.5;0.5;0.5"
-        translation_xyz="2.5;0.2;-0.2"
-      />
-      <Action
-        ID="GenerateCoveragePath"
-        vector_of_poses="{pose_stamped_vector}"
-        bottom_right_corner="{output_pose}"
-        stride_distance="0.4"
-        width="0.6"
-        height="5"
-      />
-      <Action
-        ID="VisualizePath"
-        marker_lifetime="0.000000"
-        path="{pose_stamped_vector}"
-      />
-      <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="{pose_stamped_vector}"
-        planning_group_name="manipulator"
-        position_only="true"
-        tip_link="grasp_link"
-        trajectory_sampling_rate="100"
-        velocity_scale_factor="1.000000"
-      />
-      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-        goal_duration_tolerance="-1.000000"
-        goal_position_tolerance="0.000000"
-        goal_time_tolerance="0.000000"
-        joint_trajectory_msg="{joint_trajectory_msg}"
-      />
+      <SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0" waypoint_name="Look at Airfoil"/>
+      <SubTree ID="Estimate Object Pose" _collapsed="true" guess_orientation="0;0;0.707;0.707" guess_position="4.5;0;0.5" icp_max_correspondence_distance="0.5" object_model_file_path="~/user_ws/install/mobile_manipulation_config/share/mobile_manipulation_config/description/assets/airfoil.stl" stamped_pose="{stamped_pose}"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{stamped_pose}"/>
+      <Action ID="TransformPose" input_pose="{stamped_pose}" output_pose="{output_pose}" quaternion_xyzw="0.5;-0.5;0.5;0.5" translation_xyz="2.5;0.2;-0.2"/>
+      <Action ID="GenerateCoveragePath" vector_of_poses="{pose_stamped_vector}" bottom_right_corner="{output_pose}" stride_distance="0.4" width="0.6" height="5"/>
+      <Action ID="VisualizePath" marker_lifetime="0.000000" path="{pose_stamped_vector}"/>
+      <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="{pose_stamped_vector}" planning_group_name="manipulator" position_only="true" tip_link="grasp_link" trajectory_sampling_rate="100" velocity_scale_factor="1.000000"/>
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Scan and Plan">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Basic Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/take_snapshot.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/take_snapshot.xml
index c6f9044b..307b88ac 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/take_snapshot.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/take_snapshot.xml
@@ -1,34 +1,18 @@
+<?xml version='1.0' encoding='utf-8'?>
 <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="false"
-  >
+  <BehaviorTree ID="Take Snapshot" _description="Take a point cloud snapshot and add to collision scene" _favorite="false">
     <Control ID="Sequence">
-      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
-      <Action
-        ID="GetPointCloud"
-        topic_name="/wrist_camera/points"
-        message_out="{point_cloud}"
-        timeout_sec="15.000000"
-      />
-      <Action
-        ID="UpdatePlanningSceneService"
-        point_cloud="{point_cloud}"
-        point_cloud_service="/point_cloud_service"
-      />
-      <Action
-        ID="SendPointCloudToUI"
-        point_cloud="{point_cloud}"
-        pcd_topic="/pcd_pointcloud_captures"
-      />
+      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
+      <Action ID="GetPointCloud" topic_name="/wrist_camera/points" message_out="{point_cloud}" timeout_sec="15.000000"/>
+      <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Take Snapshot">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/package.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/package.xml
index 2bd9aa35..faa76141 100644
--- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/package.xml
+++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>mobile_manipulation_config</name>
   <version>6.5.0</version>
diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/description/scene.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/description/scene.xml
index d5802714..08a226b8 100644
--- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/description/scene.xml
+++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/description/scene.xml
@@ -1,35 +1,19 @@
+<?xml version='1.0' encoding='utf-8'?>
 <!-- UR5e inspired from https://github.com/google-deepmind/mujoco_menagerie/tree/main/universal_robots_ur5e -->
 <mujoco model="ur5e scene">
-  <include file="ur5e/ur5e_globals.xml" />
-  <include file="lab_desk/desk_globals.xml" />
-  <include file="robotiq_2f85/robotiq2f85_globals.xml" />
-  <include file="ridgeback/ridgeback_globals.xml" />
+  <include file="ur5e/ur5e_globals.xml"/>
+  <include file="lab_desk/desk_globals.xml"/>
+  <include file="robotiq_2f85/robotiq2f85_globals.xml"/>
+  <include file="ridgeback/ridgeback_globals.xml"/>
 
-  <compiler angle="radian" autolimits="true" />
-  <statistic center="0.3 0 0.4" extent="0.8" />
+  <compiler angle="radian" autolimits="true"/>
+  <statistic center="0.3 0 0.4" extent="0.8"/>
 
   <worldbody>
-    <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
-    <geom
-      name="floor"
-      pos="0 0 -0.04"
-      size="0 0 0.05"
-      type="plane"
-      material="groundplane"
-    />
-    <site
-      name="scene_camera_optical_frame"
-      pos="0.0 -2.0 3.5"
-      euler="3.64 0.0 0.0"
-    />
-    <camera
-      name="scene_camera"
-      pos="0.5 -4.0 3.5"
-      fovy="58"
-      mode="fixed"
-      resolution="640 480"
-      euler="0.8 -0.4 -0.2"
-    />
-    <include file="ridgeback/ridgeback.xml" />
+    <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
+    <geom name="floor" pos="0 0 -0.04" size="0 0 0.05" type="plane" material="groundplane"/>
+    <site name="scene_camera_optical_frame" pos="0.0 -2.0 3.5" euler="3.64 0.0 0.0"/>
+    <camera name="scene_camera" pos="0.5 -4.0 3.5" fovy="58" mode="fixed" resolution="640 480" euler="0.8 -0.4 -0.2"/>
+    <include file="ridgeback/ridgeback.xml"/>
   </worldbody>
 </mujoco>
diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/description/ur5e.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/description/ur5e.xml
index b1d70c26..017dd33a 100644
--- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/description/ur5e.xml
+++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/description/ur5e.xml
@@ -1,739 +1,287 @@
+<?xml version='1.0' encoding='utf-8'?>
 <mujoco model="ur5e">
-  <compiler angle="radian" meshdir="assets" autolimits="true" />
+  <compiler angle="radian" meshdir="assets" autolimits="true"/>
 
-  <option integrator="implicitfast" />
+  <option integrator="implicitfast"/>
 
   <default>
     <default class="ur5e">
-      <material specular="0.5" shininess="0.25" />
-      <joint axis="0 1 0" range="-6.28319 6.28319" armature="0.1" />
-      <general
-        gaintype="fixed"
-        biastype="affine"
-        ctrlrange="-6.2831 6.2831"
-        gainprm="2000"
-        biasprm="0 -2000 -400"
-        forcerange="-150 150"
-      />
+      <material specular="0.5" shininess="0.25"/>
+      <joint axis="0 1 0" range="-6.28319 6.28319" armature="0.1"/>
+      <general gaintype="fixed" biastype="affine" ctrlrange="-6.2831 6.2831" gainprm="2000" biasprm="0 -2000 -400" forcerange="-150 150"/>
       <default class="size3">
         <default class="size3_limited">
-          <joint range="-3.1415 3.1415" />
-          <general ctrlrange="-3.1415 3.1415" />
+          <joint range="-3.1415 3.1415"/>
+          <general ctrlrange="-3.1415 3.1415"/>
         </default>
       </default>
       <default class="visual">
-        <geom type="mesh" contype="0" conaffinity="0" group="2" />
+        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
       </default>
       <default class="collision">
-        <geom type="capsule" group="3" />
+        <geom type="capsule" group="3"/>
         <default class="eef_collision">
-          <geom type="cylinder" />
+          <geom type="cylinder"/>
         </default>
       </default>
-      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4" />
+      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
     </default>
 
     <!-- robotiq 2f85 defaults -->
     <default class="2f85">
-      <mesh scale="0.001 0.001 0.001" />
-      <general biastype="affine" />
+      <mesh scale="0.001 0.001 0.001"/>
+      <general biastype="affine"/>
 
-      <joint axis="1 0 0" />
+      <joint axis="1 0 0"/>
       <default class="knuckle_link">
-        <joint
-          range="0 0.8"
-          armature="0.005"
-          damping="0.1"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="0 0.8" armature="0.005" damping="0.1" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
       <default class="finger_tip_link">
-        <joint
-          range="-0.872664 0.872664"
-          armature="0.001"
-          pos="0 -0.018 0.0065"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="-0.872664 0.872664" armature="0.001" pos="0 -0.018 0.0065" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
       <default class="inner_knuckle_link">
-        <joint
-          range="-0.29670597283 0.8"
-          armature="0.001"
-          stiffness="0.05"
-          springref="2.62"
-          damping="0.00125"
-        />
+        <joint range="-0.29670597283 0.8" armature="0.001" stiffness="0.05" springref="2.62" damping="0.00125"/>
       </default>
       <default class="finger_link">
-        <joint
-          range="-1.57 0"
-          armature="0.001"
-          solimplimit="0.95 0.99 0.001"
-          solreflimit="0.005 1"
-        />
+        <joint range="-1.57 0" armature="0.001" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
       </default>
 
       <default class="2f85_visual">
-        <geom type="mesh" contype="0" conaffinity="0" group="2" />
+        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
       </default>
       <default class="2f85_collision">
-        <geom type="mesh" group="3" condim="6" />
+        <geom type="mesh" group="3" condim="6"/>
         <default class="pad_box1">
-          <geom
-            mass="0"
-            type="box"
-            pos="0 -0.0026 0.028125"
-            size="0.011 0.004 0.009375"
-            friction="0.7"
-            solimp="0.95 0.99 0.001"
-            solref="0.004 1"
-            priority="1"
-            rgba="0.55 0.55 0.55 1"
-          />
+          <geom mass="0" type="box" pos="0 -0.0026 0.028125" size="0.011 0.004 0.009375" friction="0.7" solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.55 0.55 0.55 1"/>
         </default>
         <default class="pad_box2">
-          <geom
-            mass="0"
-            type="box"
-            pos="0 -0.0026 0.009375"
-            size="0.011 0.004 0.009375"
-            friction="0.6"
-            solimp="0.95 0.99 0.001"
-            solref="0.004 1"
-            priority="1"
-            rgba="0.45 0.45 0.45 1"
-          />
+          <geom mass="0" type="box" pos="0 -0.0026 0.009375" size="0.011 0.004 0.009375" friction="0.6" solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.45 0.45 0.45 1"/>
         </default>
       </default>
     </default>
     <default class="robot">
-      <material specular="0.5" shininess="0.25" />
-      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973" />
-      <general
-        dyntype="none"
-        biastype="affine"
-        ctrlrange="-3.14159 3.14159"
-        forcerange="-50000 50000"
-      />
+      <material specular="0.5" shininess="0.25"/>
+      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
+      <general dyntype="none" biastype="affine" ctrlrange="-3.14159 3.14159" forcerange="-50000 50000"/>
       <default class="limitedJoints">
-        <joint limited="true" />
+        <joint limited="true"/>
       </default>
-      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4" />
+      <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
     </default>
     <default class="rail">
-      <joint armature="0.1" damping="1" axis="0 0 1" range="-5.0 5.0" />
-      <general
-        dyntype="none"
-        biastype="affine"
-        ctrlrange="-5.0 5.0"
-        forcerange="-50000 50000"
-      />
+      <joint armature="0.1" damping="1" axis="0 0 1" range="-5.0 5.0"/>
+      <general dyntype="none" biastype="affine" ctrlrange="-5.0 5.0" forcerange="-50000 50000"/>
     </default>
   </default>
   <asset>
-    <material name="metal" rgba="0.58 0.58 0.58 1" />
-    <material name="silicone" rgba="0.1882 0.1882 0.1882 1" />
-    <material name="white" rgba="1.0 1.0 1.0 1" />
-    <material name="gray" rgba="0.4627 0.4627 0.4627 1" />
-    <material class="ur5e" name="black" rgba="0.033 0.033 0.033 1" />
-    <material class="ur5e" name="jointgray" rgba="0.278 0.278 0.278 1" />
-    <material class="ur5e" name="linkgray" rgba="0.82 0.82 0.82 1" />
-    <material class="ur5e" name="urblue" rgba="0.49 0.678 0.8 1" />
-    <mesh name="base" file="base.stl" scale="0.001 0.001 0.001" />
-    <mesh name="base_platform" file="base_platform.stl" />
-    <mesh file="base_0.obj" />
-    <mesh file="base_1.obj" />
-    <mesh file="shoulder_0.obj" />
-    <mesh file="shoulder_1.obj" />
-    <mesh file="shoulder_2.obj" />
-    <mesh file="upperarm_0.obj" />
-    <mesh file="upperarm_1.obj" />
-    <mesh file="upperarm_2.obj" />
-    <mesh file="upperarm_3.obj" />
-    <mesh file="forearm_0.obj" />
-    <mesh file="forearm_1.obj" />
-    <mesh file="forearm_2.obj" />
-    <mesh file="forearm_3.obj" />
-    <mesh file="wrist1_0.obj" />
-    <mesh file="wrist1_1.obj" />
-    <mesh file="wrist1_2.obj" />
-    <mesh file="wrist2_0.obj" />
-    <mesh file="wrist2_1.obj" />
-    <mesh file="wrist2_2.obj" />
-    <mesh file="wrist3.obj" />
+    <material name="metal" rgba="0.58 0.58 0.58 1"/>
+    <material name="silicone" rgba="0.1882 0.1882 0.1882 1"/>
+    <material name="white" rgba="1.0 1.0 1.0 1"/>
+    <material name="gray" rgba="0.4627 0.4627 0.4627 1"/>
+    <material class="ur5e" name="black" rgba="0.033 0.033 0.033 1"/>
+    <material class="ur5e" name="jointgray" rgba="0.278 0.278 0.278 1"/>
+    <material class="ur5e" name="linkgray" rgba="0.82 0.82 0.82 1"/>
+    <material class="ur5e" name="urblue" rgba="0.49 0.678 0.8 1"/>
+    <mesh name="base" file="base.stl" scale="0.001 0.001 0.001"/>
+    <mesh name="base_platform" file="base_platform.stl"/>
+    <mesh file="base_0.obj"/>
+    <mesh file="base_1.obj"/>
+    <mesh file="shoulder_0.obj"/>
+    <mesh file="shoulder_1.obj"/>
+    <mesh file="shoulder_2.obj"/>
+    <mesh file="upperarm_0.obj"/>
+    <mesh file="upperarm_1.obj"/>
+    <mesh file="upperarm_2.obj"/>
+    <mesh file="upperarm_3.obj"/>
+    <mesh file="forearm_0.obj"/>
+    <mesh file="forearm_1.obj"/>
+    <mesh file="forearm_2.obj"/>
+    <mesh file="forearm_3.obj"/>
+    <mesh file="wrist1_0.obj"/>
+    <mesh file="wrist1_1.obj"/>
+    <mesh file="wrist1_2.obj"/>
+    <mesh file="wrist2_0.obj"/>
+    <mesh file="wrist2_1.obj"/>
+    <mesh file="wrist2_2.obj"/>
+    <mesh file="wrist3.obj"/>
     <!-- Robotiq 2f85 meshes -->
-    <mesh name="robotiq_85_base" class="2f85" file="robotiq_2f85/base.stl" />
-    <mesh
-      name="robotiq_85_knuckle_link"
-      class="2f85"
-      file="robotiq_2f85/knuckle_link.stl"
-    />
-    <mesh
-      name="robotiq_85_inner_knuckle_link"
-      class="2f85"
-      file="robotiq_2f85/inner_knuckle_link.stl"
-    />
-    <mesh
-      name="robotiq_85_finger_link"
-      class="2f85"
-      file="robotiq_2f85/finger_link.stl"
-    />
-    <mesh
-      name="robotiq_85_finger_tip_link"
-      class="2f85"
-      file="robotiq_2f85/finger_tip_link.stl"
-    />
-    <mesh name="robotiq_85_pad" class="2f85" file="robotiq_2f85/pad.stl" />
-    <mesh
-      name="robotiq_85_silicone_pad"
-      class="2f85"
-      file="robotiq_2f85/silicone_pad.stl"
-    />
+    <mesh name="robotiq_85_base" class="2f85" file="robotiq_2f85/base.stl"/>
+    <mesh name="robotiq_85_knuckle_link" class="2f85" file="robotiq_2f85/knuckle_link.stl"/>
+    <mesh name="robotiq_85_inner_knuckle_link" class="2f85" file="robotiq_2f85/inner_knuckle_link.stl"/>
+    <mesh name="robotiq_85_finger_link" class="2f85" file="robotiq_2f85/finger_link.stl"/>
+    <mesh name="robotiq_85_finger_tip_link" class="2f85" file="robotiq_2f85/finger_tip_link.stl"/>
+    <mesh name="robotiq_85_pad" class="2f85" file="robotiq_2f85/pad.stl"/>
+    <mesh name="robotiq_85_silicone_pad" class="2f85" file="robotiq_2f85/silicone_pad.stl"/>
     <!-- Ridgeback -->
-    <mesh name="chassis_link" file="ridgeback/body.stl" />
-    <mesh name="top_link" file="ridgeback/top.stl" />
+    <mesh name="chassis_link" file="ridgeback/body.stl"/>
+    <mesh name="top_link" file="ridgeback/top.stl"/>
   </asset>
 
   <worldbody>
-    <light
-      name="spotlight"
-      mode="targetbodycom"
-      target="wrist_2_link"
-      pos="0 -1 2"
-    />
+    <light name="spotlight" mode="targetbodycom" target="wrist_2_link" pos="0 -1 2"/>
 
-    <site
-      name="scene_camera_optical_frame"
-      pos="0.0 -2.0 3.5"
-      euler="3.64 0.0 0.0"
-    />
-    <camera
-      name="scene_camera"
-      pos="0.5 -4.0 3.5"
-      fovy="58"
-      mode="fixed"
-      resolution="640 480"
-      euler="0.8 -0.4 -0.2"
-    />
+    <site name="scene_camera_optical_frame" pos="0.0 -2.0 3.5" euler="3.64 0.0 0.0"/>
+    <camera name="scene_camera" pos="0.5 -4.0 3.5" fovy="58" mode="fixed" resolution="640 480" euler="0.8 -0.4 -0.2"/>
     <body name="base_platform_rotation" pos="0 0 0" euler="0 0 0">
-      <inertial mass="10.0" pos="0 0 0" diaginertia="0.001 0.001 0.001" />
+      <inertial mass="10.0" pos="0 0 0" diaginertia="0.001 0.001 0.001"/>
       <body name="base_platform" pos="0 0 0" euler="0 0 0">
         <body name="ridgeback_base_link">
-          <geom
-            name="chassis_link"
-            mesh="chassis_link"
-            material="black"
-            type="mesh"
-            contype="0"
-            conaffinity="0"
-            group="2"
-          />
-          <geom
-            name="riser_link"
-            size="0.2465 0.2465 0.0275"
-            pos="0 0 0.2475"
-            quat="0.92388 0 0 0.382683"
-            type="box"
-            rgba="0.4 0.4 0.4 1"
-          />
-          <geom
-            name="top_link"
-            mesh="top_link"
-            material="black"
-            type="mesh"
-            contype="0"
-            conaffinity="0"
-            group="2"
-          />
+          <geom name="chassis_link" mesh="chassis_link" material="black" type="mesh" contype="0" conaffinity="0" group="2"/>
+          <geom name="riser_link" size="0.2465 0.2465 0.0275" pos="0 0 0.2475" quat="0.92388 0 0 0.382683" type="box" rgba="0.4 0.4 0.4 1"/>
+          <geom name="top_link" mesh="top_link" material="black" type="mesh" contype="0" conaffinity="0" group="2"/>
           <body name="front_rocker_link" pos="0.319 0 0.05">
-            <inertial
-              pos="0 0 0"
-              quat="0.475329 0.523511 0.47531 0.523526"
-              mass="10.267"
-              diaginertia="0.432583 0.412817 0.0288"
-            />
-            <joint
-              name="front_rocker"
-              pos="0 0 0"
-              axis="1 0 0"
-              range="-0.08726 0.08726"
-            />
+            <inertial pos="0 0 0" quat="0.475329 0.523511 0.47531 0.523526" mass="10.267" diaginertia="0.432583 0.412817 0.0288"/>
+            <joint name="front_rocker" pos="0 0 0" axis="1 0 0" range="-0.08726 0.08726"/>
             <body name="front_left_wheel_link" pos="0 0.2755 0">
-              <inertial
-                pos="0 0 0"
-                quat="0.707107 0 0 0.707107"
-                mass="2.3"
-                diaginertia="0.0066424 0.0033212 0.0033212"
-              />
-              <joint name="front_left_wheel" pos="0 0 0" axis="0 1 0" />
-              <geom
-                name="front_left_wheel_link"
-                size="0.0759 0.0395"
-                quat="0.707107 0.707107 0 0"
-                type="cylinder"
-                rgba="0.15 0.15 0.15 1"
-              />
+              <inertial pos="0 0 0" quat="0.707107 0 0 0.707107" mass="2.3" diaginertia="0.0066424 0.0033212 0.0033212"/>
+              <joint name="front_left_wheel" pos="0 0 0" axis="0 1 0"/>
+              <geom name="front_left_wheel_link" size="0.0759 0.0395" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.15 0.15 0.15 1"/>
             </body>
             <body name="front_right_wheel_link" pos="0 -0.2755 0">
-              <inertial
-                pos="0 0 0"
-                quat="0.707107 0 0 0.707107"
-                mass="2.3"
-                diaginertia="0.0066424 0.0033212 0.0033212"
-              />
-              <joint name="front_right_wheel" pos="0 0 0" axis="0 1 0" />
-              <geom
-                name="front_right_wheel_link"
-                size="0.0759 0.0395"
-                quat="0.707107 0.707107 0 0"
-                type="cylinder"
-                rgba="0.15 0.15 0.15 1"
-              />
+              <inertial pos="0 0 0" quat="0.707107 0 0 0.707107" mass="2.3" diaginertia="0.0066424 0.0033212 0.0033212"/>
+              <joint name="front_right_wheel" pos="0 0 0" axis="0 1 0"/>
+              <geom name="front_right_wheel_link" size="0.0759 0.0395" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.15 0.15 0.15 1"/>
             </body>
           </body>
           <body name="rear_left_wheel_link" pos="-0.319 0.2755 0.05">
-            <inertial
-              pos="0 0 0"
-              quat="0.707107 0 0 0.707107"
-              mass="2.3"
-              diaginertia="0.0066424 0.0033212 0.0033212"
-            />
-            <joint name="rear_left_wheel" pos="0 0 0" axis="0 1 0" />
-            <geom
-              name="rear_left_wheel_link"
-              size="0.0759 0.0395"
-              quat="0.707107 0.707107 0 0"
-              type="cylinder"
-              rgba="0.15 0.15 0.15 1"
-            />
+            <inertial pos="0 0 0" quat="0.707107 0 0 0.707107" mass="2.3" diaginertia="0.0066424 0.0033212 0.0033212"/>
+            <joint name="rear_left_wheel" pos="0 0 0" axis="0 1 0"/>
+            <geom name="rear_left_wheel_link" size="0.0759 0.0395" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.15 0.15 0.15 1"/>
           </body>
           <body name="rear_right_wheel_link" pos="-0.319 -0.2755 0.05">
-            <inertial
-              pos="0 0 0"
-              quat="0.707107 0 0 0.707107"
-              mass="2.3"
-              diaginertia="0.0066424 0.0033212 0.0033212"
-            />
-            <joint name="rear_right_wheel" pos="0 0 0" axis="0 1 0" />
-            <geom
-              name="rear_right_wheel_link"
-              size="0.0759 0.0395"
-              quat="0.707107 0.707107 0 0"
-              type="cylinder"
-              rgba="0.15 0.15 0.15 1"
-            />
+            <inertial pos="0 0 0" quat="0.707107 0 0 0.707107" mass="2.3" diaginertia="0.0066424 0.0033212 0.0033212"/>
+            <joint name="rear_right_wheel" pos="0 0 0" axis="0 1 0"/>
+            <geom name="rear_right_wheel_link" size="0.0759 0.0395" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.15 0.15 0.15 1"/>
           </body>
-          <joint
-            name="linear_x_joint"
-            type="slide"
-            axis="1 0 0"
-            range="-5.0 5.0"
-          />
-          <joint
-            name="linear_y_joint"
-            type="slide"
-            axis="0 1 0"
-            range="-5.0 5.0"
-          />
-          <joint name="rotational_yaw_joint" axis="0 0 1" />
+          <joint name="linear_x_joint" type="slide" axis="1 0 0" range="-5.0 5.0"/>
+          <joint name="linear_y_joint" type="slide" axis="0 1 0" range="-5.0 5.0"/>
+          <joint name="rotational_yaw_joint" axis="0 0 1"/>
           <body name="base" pos="0.0 0.0 0.28" euler="0 0 0" childclass="ur5e">
-            <inertial
-              mass="4.0"
-              pos="0 0 0"
-              diaginertia="0.00443333156 0.00443333156 0.0072"
-            />
-            <geom mesh="base_0" material="black" class="visual" />
-            <geom mesh="base_1" material="jointgray" class="visual" />
+            <inertial mass="4.0" pos="0 0 0" diaginertia="0.00443333156 0.00443333156 0.0072"/>
+            <geom mesh="base_0" material="black" class="visual"/>
+            <geom mesh="base_1" material="jointgray" class="visual"/>
             <body name="shoulder_link" pos="0 0 0.163">
-              <inertial
-                mass="3.7"
-                pos="0 0 0"
-                diaginertia="0.0102675 0.0102675 0.00666"
-              />
-              <joint name="shoulder_pan_joint" class="size3" axis="0 0 1" />
-              <geom mesh="shoulder_0" material="urblue" class="visual" />
-              <geom mesh="shoulder_1" material="black" class="visual" />
-              <geom mesh="shoulder_2" material="jointgray" class="visual" />
-              <geom class="collision" size="0.06 0.06" pos="0 0 -0.04" />
+              <inertial mass="3.7" pos="0 0 0" diaginertia="0.0102675 0.0102675 0.00666"/>
+              <joint name="shoulder_pan_joint" class="size3" axis="0 0 1"/>
+              <geom mesh="shoulder_0" material="urblue" class="visual"/>
+              <geom mesh="shoulder_1" material="black" class="visual"/>
+              <geom mesh="shoulder_2" material="jointgray" class="visual"/>
+              <geom class="collision" size="0.06 0.06" pos="0 0 -0.04"/>
               <body name="upper_arm_link" pos="0 0.138 0" quat="1 0 1 0">
-                <inertial
-                  mass="8.393"
-                  pos="0 0 0.2125"
-                  diaginertia="0.133886 0.133886 0.0151074"
-                />
-                <joint name="shoulder_lift_joint" class="size3" axis="0 1 0" />
-                <geom mesh="upperarm_0" material="linkgray" class="visual" />
-                <geom mesh="upperarm_1" material="black" class="visual" />
-                <geom mesh="upperarm_2" material="jointgray" class="visual" />
-                <geom mesh="upperarm_3" material="urblue" class="visual" />
-                <geom
-                  class="collision"
-                  pos="0 -0.04 0"
-                  quat="1 1 0 0"
-                  size="0.06 0.06"
-                />
-                <geom class="collision" size="0.05 0.2" pos="0 0 0.2" />
+                <inertial mass="8.393" pos="0 0 0.2125" diaginertia="0.133886 0.133886 0.0151074"/>
+                <joint name="shoulder_lift_joint" class="size3" axis="0 1 0"/>
+                <geom mesh="upperarm_0" material="linkgray" class="visual"/>
+                <geom mesh="upperarm_1" material="black" class="visual"/>
+                <geom mesh="upperarm_2" material="jointgray" class="visual"/>
+                <geom mesh="upperarm_3" material="urblue" class="visual"/>
+                <geom class="collision" pos="0 -0.04 0" quat="1 1 0 0" size="0.06 0.06"/>
+                <geom class="collision" size="0.05 0.2" pos="0 0 0.2"/>
                 <body name="forearm_link" pos="0 -0.131 0.425">
-                  <inertial
-                    mass="2.275"
-                    pos="0 0 0.196"
-                    diaginertia="0.0311796 0.0311796 0.004095"
-                  />
-                  <joint name="elbow_joint" class="size3_limited" />
-                  <geom mesh="forearm_0" material="urblue" class="visual" />
-                  <geom mesh="forearm_1" material="linkgray" class="visual" />
-                  <geom mesh="forearm_2" material="black" class="visual" />
-                  <geom mesh="forearm_3" material="jointgray" class="visual" />
-                  <geom
-                    class="collision"
-                    pos="0 0.08 0"
-                    quat="1 1 0 0"
-                    size="0.055 0.06"
-                  />
-                  <geom class="collision" size="0.038 0.19" pos="0 0 0.2" />
+                  <inertial mass="2.275" pos="0 0 0.196" diaginertia="0.0311796 0.0311796 0.004095"/>
+                  <joint name="elbow_joint" class="size3_limited"/>
+                  <geom mesh="forearm_0" material="urblue" class="visual"/>
+                  <geom mesh="forearm_1" material="linkgray" class="visual"/>
+                  <geom mesh="forearm_2" material="black" class="visual"/>
+                  <geom mesh="forearm_3" material="jointgray" class="visual"/>
+                  <geom class="collision" pos="0 0.08 0" quat="1 1 0 0" size="0.055 0.06"/>
+                  <geom class="collision" size="0.038 0.19" pos="0 0 0.2"/>
                   <body name="wrist_1_link" pos="0 0 0.392" quat="1 0 1 0">
-                    <inertial
-                      mass="1.219"
-                      pos="0 0.127 0"
-                      diaginertia="0.0025599 0.0025599 0.0021942"
-                    />
-                    <joint name="wrist_1_joint" class="size3" />
-                    <geom mesh="wrist1_0" material="black" class="visual" />
-                    <geom mesh="wrist1_1" material="urblue" class="visual" />
-                    <geom mesh="wrist1_2" material="jointgray" class="visual" />
-                    <geom
-                      class="collision"
-                      pos="0 0.05 0"
-                      quat="1 1 0 0"
-                      size="0.04 0.07"
-                    />
+                    <inertial mass="1.219" pos="0 0.127 0" diaginertia="0.0025599 0.0025599 0.0021942"/>
+                    <joint name="wrist_1_joint" class="size3"/>
+                    <geom mesh="wrist1_0" material="black" class="visual"/>
+                    <geom mesh="wrist1_1" material="urblue" class="visual"/>
+                    <geom mesh="wrist1_2" material="jointgray" class="visual"/>
+                    <geom class="collision" pos="0 0.05 0" quat="1 1 0 0" size="0.04 0.07"/>
                     <body name="wrist_2_link" pos="0 0.127 0">
-                      <inertial
-                        mass="1.219"
-                        pos="0 0 0.1"
-                        diaginertia="0.0025599 0.0025599 0.0021942"
-                      />
-                      <joint name="wrist_2_joint" axis="0 0 1" class="size3" />
-                      <geom mesh="wrist2_0" material="black" class="visual" />
-                      <geom mesh="wrist2_1" material="urblue" class="visual" />
-                      <geom
-                        mesh="wrist2_2"
-                        material="jointgray"
-                        class="visual"
-                      />
-                      <geom class="collision" size="0.04 0.06" pos="0 0 0.04" />
-                      <geom
-                        class="collision"
-                        pos="0 0.02 0.1"
-                        quat="1 1 0 0"
-                        size="0.04 0.04"
-                      />
+                      <inertial mass="1.219" pos="0 0 0.1" diaginertia="0.0025599 0.0025599 0.0021942"/>
+                      <joint name="wrist_2_joint" axis="0 0 1" class="size3"/>
+                      <geom mesh="wrist2_0" material="black" class="visual"/>
+                      <geom mesh="wrist2_1" material="urblue" class="visual"/>
+                      <geom mesh="wrist2_2" material="jointgray" class="visual"/>
+                      <geom class="collision" size="0.04 0.06" pos="0 0 0.04"/>
+                      <geom class="collision" pos="0 0.02 0.1" quat="1 1 0 0" size="0.04 0.04"/>
                       <body name="wrist_3_link" pos="0 0 0.1">
-                        <inertial
-                          mass="0.1889"
-                          pos="0 0.0771683 0"
-                          quat="1 0 0 1"
-                          diaginertia="0.000132134 9.90863e-05 9.90863e-05"
-                        />
-                        <joint name="wrist_3_joint" class="size3" />
-                        <geom
-                          material="linkgray"
-                          mesh="wrist3"
-                          class="visual"
-                        />
+                        <inertial mass="0.1889" pos="0 0.0771683 0" quat="1 0 0 1" diaginertia="0.000132134 9.90863e-05 9.90863e-05"/>
+                        <joint name="wrist_3_joint" class="size3"/>
+                        <geom material="linkgray" mesh="wrist3" class="visual"/>
                         <!-- <geom class="eef_collision" pos="0 0.08 0" quat="1 1 0 0" size="0.04 0.02"/> -->
-                        <site
-                          name="attachment_site"
-                          pos="0 0.1 0"
-                          quat="-1 1 0 0"
-                        />
-                        <camera
-                          name="wrist_camera"
-                          pos="0.0 0.2 0.05"
-                          fovy="58"
-                          mode="fixed"
-                          resolution="640 480"
-                          euler="1.57 0.0 0.0"
-                        />
-                        <site
-                          name="wrist_camera_optical_frame"
-                          pos="0.0 0.2 0.05"
-                          euler="-1.57 0 0"
-                        />
+                        <site name="attachment_site" pos="0 0.1 0" quat="-1 1 0 0"/>
+                        <camera name="wrist_camera" pos="0.0 0.2 0.05" fovy="58" mode="fixed" resolution="640 480" euler="1.57 0.0 0.0"/>
+                        <site name="wrist_camera_optical_frame" pos="0.0 0.2 0.05" euler="-1.57 0 0"/>
                         <body childclass="2f85">
                           <!-- site for end effector's f/t sensor right at the base of the robotiq gripper -->
-                          <site
-                            name="tcp_fts_sensor"
-                            size="0.01"
-                            pos="0.001 0.1 0"
-                            quat="-1 1 0 0"
-                          />
+                          <site name="tcp_fts_sensor" size="0.01" pos="0.001 0.1 0" quat="-1 1 0 0"/>
                           1
                           <!-- Right-hand side 4-bar linkage -->
-                          <body
-                            name="robotiq_85_base_link"
-                            gravcomp="1"
-                            pos="0.0 0.1441 0"
-                            quat="0.5 -0.5 0.5 0.5"
-                          >
-                            <inertial
-                              mass="0.777441"
-                              pos="0 -2.70394e-05 0.0354675"
-                              quat="1 -0.00152849 0 0"
-                              diaginertia="0.000260285 0.000225381 0.000152708"
-                            />
-                            <geom
-                              class="visual"
-                              mesh="robotiq_85_base"
-                              material="black"
-                            />
-                            <geom class="collision" mesh="robotiq_85_base" />
-                            <site
-                              name="pinch"
-                              pos="0 0 0.145"
-                              type="sphere"
-                              group="5"
-                              rgba="0.9 0.9 0.9 1"
-                              size="0.005"
-                            />
+                          <body name="robotiq_85_base_link" gravcomp="1" pos="0.0 0.1441 0" quat="0.5 -0.5 0.5 0.5">
+                            <inertial mass="0.777441" pos="0 -2.70394e-05 0.0354675" quat="1 -0.00152849 0 0" diaginertia="0.000260285 0.000225381 0.000152708"/>
+                            <geom class="visual" mesh="robotiq_85_base" material="black"/>
+                            <geom class="collision" mesh="robotiq_85_base"/>
+                            <site name="pinch" pos="0 0 0.145" type="sphere" group="5" rgba="0.9 0.9 0.9 1" size="0.005"/>
                             <!-- Right-hand side 4-bar linkage -->
-                            <body
-                              name="robotiq_85_right_knuckle_link"
-                              gravcomp="1"
-                              pos="0 0.0306011 0.054904"
-                            >
-                              <inertial
-                                mass="0.00899563"
-                                pos="2.96931e-12 0.0177547 0.00107314"
-                                quat="0.681301 0.732003 0 0"
-                                diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"
-                              />
-                              <joint
-                                name="robotiq_85_right_knuckle_joint"
-                                class="knuckle_link"
-                              />
-                              <geom
-                                class="visual"
-                                mesh="robotiq_85_knuckle_link"
-                                material="gray"
-                              />
-                              <geom
-                                class="collision"
-                                mesh="robotiq_85_knuckle_link"
-                              />
-                              <body
-                                name="robotiq_85_right_finger_link"
-                                gravcomp="1"
-                                pos="0 0.0315 -0.0041"
-                              >
-                                <inertial
-                                  mass="0.0140974"
-                                  pos="0 0.00301209 0.0232175"
-                                  quat="0.705636 -0.0455904 0.0455904 0.705636"
-                                  diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"
-                                />
-                                <joint
-                                  name="robotiq_85_right_finger_joint"
-                                  class="finger_link"
-                                />
-                                <geom
-                                  class="visual"
-                                  mesh="robotiq_85_finger_link"
-                                  material="black"
-                                />
-                                <geom
-                                  class="collision"
-                                  mesh="robotiq_85_finger_link"
-                                />
+                            <body name="robotiq_85_right_knuckle_link" gravcomp="1" pos="0 0.0306011 0.054904">
+                              <inertial mass="0.00899563" pos="2.96931e-12 0.0177547 0.00107314" quat="0.681301 0.732003 0 0" diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
+                              <joint name="robotiq_85_right_knuckle_joint" class="knuckle_link"/>
+                              <geom class="visual" mesh="robotiq_85_knuckle_link" material="gray"/>
+                              <geom class="collision" mesh="robotiq_85_knuckle_link"/>
+                              <body name="robotiq_85_right_finger_link" gravcomp="1" pos="0 0.0315 -0.0041">
+                                <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636" diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
+                                <joint name="robotiq_85_right_finger_joint" class="finger_link"/>
+                                <geom class="visual" mesh="robotiq_85_finger_link" material="black"/>
+                                <geom class="collision" mesh="robotiq_85_finger_link"/>
                               </body>
                             </body>
-                            <body
-                              name="robotiq_85_right_inner_knuckle_link"
-                              gravcomp="1"
-                              pos="0 0.0132 0.0609"
-                            >
-                              <inertial
-                                mass="0.0221642"
-                                pos="-8.65005e-09 0.0181624 0.0212658"
-                                quat="0.663403 -0.244737 0.244737 0.663403"
-                                diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"
-                              />
-                              <joint
-                                name="robotiq_85_right_inner_knuckle_joint"
-                                class="inner_knuckle_link"
-                              />
-                              <geom
-                                class="visual"
-                                mesh="robotiq_85_inner_knuckle_link"
-                                material="black"
-                              />
-                              <geom
-                                class="collision"
-                                mesh="robotiq_85_inner_knuckle_link"
-                              />
-                              <body
-                                name="robotiq_85_right_finger_tip_link"
-                                gravcomp="1"
-                                pos="0 0.055 0.0375"
-                              >
-                                <inertial
-                                  mass="0.0125222"
-                                  pos="0 -0.011046 0.0124786"
-                                  quat="1 0.1664 0 0"
-                                  diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"
-                                />
-                                <joint
-                                  name="robotiq_85_right_finger_tip_joint"
-                                  class="finger_tip_link"
-                                />
-                                <geom
-                                  class="visual"
-                                  mesh="robotiq_85_finger_tip_link"
-                                  material="black"
-                                />
-                                <geom
-                                  class="collision"
-                                  mesh="robotiq_85_finger_tip_link"
-                                />
+                            <body name="robotiq_85_right_inner_knuckle_link" gravcomp="1" pos="0 0.0132 0.0609">
+                              <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403" diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+                              <joint name="robotiq_85_right_inner_knuckle_joint" class="inner_knuckle_link"/>
+                              <geom class="visual" mesh="robotiq_85_inner_knuckle_link" material="black"/>
+                              <geom class="collision" mesh="robotiq_85_inner_knuckle_link"/>
+                              <body name="robotiq_85_right_finger_tip_link" gravcomp="1" pos="0 0.055 0.0375">
+                                <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0" diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
+                                <joint name="robotiq_85_right_finger_tip_joint" class="finger_tip_link"/>
+                                <geom class="visual" mesh="robotiq_85_finger_tip_link" material="black"/>
+                                <geom class="collision" mesh="robotiq_85_finger_tip_link"/>
                                 <body name="right_pad" pos="0 -0.0189 0.01352">
-                                  <geom class="pad_box1" name="right_pad1" />
-                                  <geom class="pad_box2" name="right_pad2" />
-                                  <inertial
-                                    mass="0.0035"
-                                    pos="0 -0.0025 0.0185"
-                                    quat="0.707107 0 0 0.707107"
-                                    diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"
-                                  />
-                                  <geom class="visual" mesh="robotiq_85_pad" />
+                                  <geom class="pad_box1" name="right_pad1"/>
+                                  <geom class="pad_box2" name="right_pad2"/>
+                                  <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="0.707107 0 0 0.707107" diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
+                                  <geom class="visual" mesh="robotiq_85_pad"/>
                                   <body name="right_silicone_pad">
-                                    <geom
-                                      class="visual"
-                                      mesh="robotiq_85_silicone_pad"
-                                      material="black"
-                                    />
+                                    <geom class="visual" mesh="robotiq_85_silicone_pad" material="black"/>
                                   </body>
                                 </body>
                               </body>
                             </body>
                             <!-- Left-hand side 4-bar linkage -->
-                            <body
-                              name="robotiq_85_left_knuckle_link"
-                              gravcomp="1"
-                              pos="0 -0.0306011 0.054904"
-                              quat="0 0 0 1"
-                            >
-                              <inertial
-                                mass="0.00899563"
-                                pos="0 0.0177547 0.00107314"
-                                quat="0.681301 0.732003 0 0"
-                                diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"
-                              />
-                              <joint
-                                name="robotiq_85_left_knuckle_joint"
-                                class="knuckle_link"
-                              />
-                              <geom
-                                class="visual"
-                                mesh="robotiq_85_knuckle_link"
-                                material="gray"
-                              />
-                              <geom
-                                class="collision"
-                                mesh="robotiq_85_knuckle_link"
-                              />
-                              <body
-                                name="robotiq_85_left_finger_link"
-                                gravcomp="1"
-                                pos="0 0.0315 -0.0041"
-                              >
-                                <inertial
-                                  mass="0.0140974"
-                                  pos="0 0.00301209 0.0232175"
-                                  quat="0.705636 -0.0455904 0.0455904 0.705636"
-                                  diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"
-                                />
-                                <joint
-                                  name="robotiq_85_left_finger_joint"
-                                  class="finger_link"
-                                />
-                                <geom
-                                  class="visual"
-                                  mesh="robotiq_85_finger_link"
-                                  material="black"
-                                />
-                                <geom
-                                  class="collision"
-                                  mesh="robotiq_85_finger_link"
-                                />
+                            <body name="robotiq_85_left_knuckle_link" gravcomp="1" pos="0 -0.0306011 0.054904" quat="0 0 0 1">
+                              <inertial mass="0.00899563" pos="0 0.0177547 0.00107314" quat="0.681301 0.732003 0 0" diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
+                              <joint name="robotiq_85_left_knuckle_joint" class="knuckle_link"/>
+                              <geom class="visual" mesh="robotiq_85_knuckle_link" material="gray"/>
+                              <geom class="collision" mesh="robotiq_85_knuckle_link"/>
+                              <body name="robotiq_85_left_finger_link" gravcomp="1" pos="0 0.0315 -0.0041">
+                                <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636" diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
+                                <joint name="robotiq_85_left_finger_joint" class="finger_link"/>
+                                <geom class="visual" mesh="robotiq_85_finger_link" material="black"/>
+                                <geom class="collision" mesh="robotiq_85_finger_link"/>
                               </body>
                             </body>
-                            <body
-                              name="robotiq_85_left_inner_knuckle_link"
-                              gravcomp="1"
-                              pos="0 -0.0132 0.0609"
-                              quat="0 0 0 1"
-                            >
-                              <inertial
-                                mass="0.0221642"
-                                pos="-8.65005e-09 0.0181624 0.0212658"
-                                quat="0.663403 -0.244737 0.244737 0.663403"
-                                diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"
-                              />
-                              <joint
-                                name="robotiq_85_left_inner_knuckle_joint"
-                                class="inner_knuckle_link"
-                              />
-                              <geom
-                                class="visual"
-                                mesh="robotiq_85_inner_knuckle_link"
-                                material="black"
-                              />
-                              <geom
-                                class="collision"
-                                mesh="robotiq_85_inner_knuckle_link"
-                              />
-                              <body
-                                name="robotiq_85_left_finger_tip_link"
-                                gravcomp="1"
-                                pos="0 0.055 0.0375"
-                              >
-                                <inertial
-                                  mass="0.0125222"
-                                  pos="0 -0.011046 0.0124786"
-                                  quat="1 0.1664 0 0"
-                                  diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"
-                                />
-                                <joint
-                                  name="robotiq_85_left_finger_tip_joint"
-                                  class="finger_tip_link"
-                                />
-                                <geom
-                                  class="visual"
-                                  mesh="robotiq_85_finger_tip_link"
-                                  material="black"
-                                />
-                                <geom
-                                  class="collision"
-                                  mesh="robotiq_85_finger_tip_link"
-                                />
+                            <body name="robotiq_85_left_inner_knuckle_link" gravcomp="1" pos="0 -0.0132 0.0609" quat="0 0 0 1">
+                              <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403" diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+                              <joint name="robotiq_85_left_inner_knuckle_joint" class="inner_knuckle_link"/>
+                              <geom class="visual" mesh="robotiq_85_inner_knuckle_link" material="black"/>
+                              <geom class="collision" mesh="robotiq_85_inner_knuckle_link"/>
+                              <body name="robotiq_85_left_finger_tip_link" gravcomp="1" pos="0 0.055 0.0375">
+                                <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0" diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
+                                <joint name="robotiq_85_left_finger_tip_joint" class="finger_tip_link"/>
+                                <geom class="visual" mesh="robotiq_85_finger_tip_link" material="black"/>
+                                <geom class="collision" mesh="robotiq_85_finger_tip_link"/>
                                 <body name="left_pad" pos="0 -0.0189 0.01352">
-                                  <geom class="pad_box1" name="left_pad1" />
-                                  <geom class="pad_box2" name="left_pad2" />
-                                  <inertial
-                                    mass="0.0035"
-                                    pos="0 -0.0025 0.0185"
-                                    quat="1 0 0 1"
-                                    diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"
-                                  />
-                                  <geom class="visual" mesh="robotiq_85_pad" />
+                                  <geom class="pad_box1" name="left_pad1"/>
+                                  <geom class="pad_box2" name="left_pad2"/>
+                                  <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="1 0 0 1" diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
+                                  <geom class="visual" mesh="robotiq_85_pad"/>
                                   <body name="left_silicone_pad">
-                                    <geom
-                                      class="visual"
-                                      mesh="robotiq_85_silicone_pad"
-                                      material="black"
-                                    />
+                                    <geom class="visual" mesh="robotiq_85_silicone_pad" material="black"/>
                                   </body>
                                 </body>
                               </body>
@@ -753,148 +301,50 @@
   </worldbody>
 
   <actuator>
-    <position
-      class="rail"
-      name="linear_x_joint"
-      joint="linear_x_joint"
-      kp="75000"
-      dampratio="1.0"
-    />
-    <position
-      class="rail"
-      name="linear_y_joint"
-      joint="linear_y_joint"
-      kp="75000"
-      dampratio="1.0"
-    />
-    <position
-      class="robot"
-      name="rotational_yaw_joint"
-      joint="rotational_yaw_joint"
-      kp="75000"
-      dampratio="1.0"
-    />
+    <position class="rail" name="linear_x_joint" joint="linear_x_joint" kp="75000" dampratio="1.0"/>
+    <position class="rail" name="linear_y_joint" joint="linear_y_joint" kp="75000" dampratio="1.0"/>
+    <position class="robot" name="rotational_yaw_joint" joint="rotational_yaw_joint" kp="75000" dampratio="1.0"/>
   </actuator>
 
   <actuator>
-    <general
-      class="size3"
-      name="shoulder_pan_joint"
-      joint="shoulder_pan_joint"
-      gainprm="75000"
-      biasprm="0 -75000 -15000"
-    />
-    <general
-      class="size3"
-      name="shoulder_lift_joint"
-      joint="shoulder_lift_joint"
-      gainprm="75000"
-      biasprm="0 -75000 -15000"
-    />
-    <general
-      class="size3_limited"
-      name="elbow_joint"
-      joint="elbow_joint"
-      gainprm="75000"
-      biasprm="0 -75000 -15000"
-    />
-    <general
-      class="size3"
-      name="wrist_1_joint"
-      joint="wrist_1_joint"
-      gainprm="50000"
-      biasprm="0 -50000 -10000"
-    />
-    <general
-      class="size3"
-      name="wrist_2_joint"
-      joint="wrist_2_joint"
-      gainprm="50000"
-      biasprm="0 -50000 -10000"
-    />
-    <general
-      class="size3"
-      name="wrist_3_joint"
-      joint="wrist_3_joint"
-      gainprm="50000"
-      biasprm="0 -50000 -10000"
-    />
-    <general
-      class="2f85"
-      name="robotiq_85_left_knuckle_joint"
-      tendon="split"
-      forcerange="-5 5"
-      ctrlrange="0 .8"
-      gainprm="100 0 0"
-      biasprm="0 -100 -150"
-    />
+    <general class="size3" name="shoulder_pan_joint" joint="shoulder_pan_joint" gainprm="75000" biasprm="0 -75000 -15000"/>
+    <general class="size3" name="shoulder_lift_joint" joint="shoulder_lift_joint" gainprm="75000" biasprm="0 -75000 -15000"/>
+    <general class="size3_limited" name="elbow_joint" joint="elbow_joint" gainprm="75000" biasprm="0 -75000 -15000"/>
+    <general class="size3" name="wrist_1_joint" joint="wrist_1_joint" gainprm="50000" biasprm="0 -50000 -10000"/>
+    <general class="size3" name="wrist_2_joint" joint="wrist_2_joint" gainprm="50000" biasprm="0 -50000 -10000"/>
+    <general class="size3" name="wrist_3_joint" joint="wrist_3_joint" gainprm="50000" biasprm="0 -50000 -10000"/>
+    <general class="2f85" name="robotiq_85_left_knuckle_joint" tendon="split" forcerange="-5 5" ctrlrange="0 .8" gainprm="100 0 0" biasprm="0 -100 -150"/>
   </actuator>
 
   <contact>
-    <exclude body1="base_platform" body2="base" />
+    <exclude body1="base_platform" body2="base"/>
   </contact>
 
   <!-- Robotiq gripper contact -->
   <contact>
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_left_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_right_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_left_inner_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_base_link"
-      body2="robotiq_85_right_inner_knuckle_link"
-    />
-    <exclude
-      body1="robotiq_85_right_finger_link"
-      body2="robotiq_85_right_finger_tip_link"
-    />
-    <exclude
-      body1="robotiq_85_left_finger_link"
-      body2="robotiq_85_left_finger_tip_link"
-    />
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_left_knuckle_link"/>
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_right_knuckle_link"/>
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_left_inner_knuckle_link"/>
+    <exclude body1="robotiq_85_base_link" body2="robotiq_85_right_inner_knuckle_link"/>
+    <exclude body1="robotiq_85_right_finger_link" body2="robotiq_85_right_finger_tip_link"/>
+    <exclude body1="robotiq_85_left_finger_link" body2="robotiq_85_left_finger_tip_link"/>
   </contact>
   <tendon>
     <fixed name="split">
-      <joint joint="robotiq_85_right_knuckle_joint" coef="0.5" />
-      <joint joint="robotiq_85_left_knuckle_joint" coef="0.5" />
+      <joint joint="robotiq_85_right_knuckle_joint" coef="0.5"/>
+      <joint joint="robotiq_85_left_knuckle_joint" coef="0.5"/>
     </fixed>
   </tendon>
 
   <equality>
-    <connect
-      anchor="0 0 0"
-      body1="robotiq_85_right_finger_tip_link"
-      body2="robotiq_85_right_finger_link"
-      solimp="0.95 0.99 0.001"
-      solref="0.005 1"
-    />
-    <connect
-      anchor="0 0 0"
-      body1="robotiq_85_left_finger_tip_link"
-      body2="robotiq_85_left_finger_link"
-      solimp="0.95 0.99 0.001"
-      solref="0.005 1"
-    />
-    <joint
-      joint1="robotiq_85_left_knuckle_joint"
-      joint2="robotiq_85_right_knuckle_joint"
-      polycoef="0 1 0 0 0"
-      solimp="0.95 0.99 0.001"
-      solref="0.005 1"
-    />
+    <connect anchor="0 0 0" body1="robotiq_85_right_finger_tip_link" body2="robotiq_85_right_finger_link" solimp="0.95 0.99 0.001" solref="0.005 1"/>
+    <connect anchor="0 0 0" body1="robotiq_85_left_finger_tip_link" body2="robotiq_85_left_finger_link" solimp="0.95 0.99 0.001" solref="0.005 1"/>
+    <joint joint1="robotiq_85_left_knuckle_joint" joint2="robotiq_85_right_knuckle_joint" polycoef="0 1 0 0 0" solimp="0.95 0.99 0.001" solref="0.005 1"/>
   </equality>
 
   <sensor>
-    <force site="tcp_fts_sensor" />
-    <torque site="tcp_fts_sensor" />
+    <force site="tcp_fts_sensor"/>
+    <torque site="tcp_fts_sensor"/>
   </sensor>
 
   <keyframe>
@@ -904,15 +354,6 @@
     <!-- right_knuckle, right_finger, right_inner_knuckle, right_finger_tip, left_knuckle, left_finger, left_inner_knuckle, left_finger_tip -->
     <!-- ctrl -->
     <!-- linear_x_joint, linear_y_joint, rotational_yaw_joint, shoulder_pan, shoulder_lift, elbow, wrist_1, wrist_2, wrist_3, robotiq_85_left_knuckle_joint -->
-    <key
-      qpos="
-      0 0 0 0 0
-      0 0 0 0 -1.534 0.8197 -2.2686 -1.57076 0
-      0 0 0 0 0 0 0 0
-    "
-      ctrl="
-      0 0 0 0 -1.534 0.8197 -2.2686 -1.57076 0 0
-    "
-    />
+    <key qpos="       0 0 0 0 0       0 0 0 0 -1.534 0.8197 -2.2686 -1.57076 0       0 0 0 0 0 0 0 0     " ctrl="       0 0 0 0 -1.534 0.8197 -2.2686 -1.57076 0 0     "/>
   </keyframe>
 </mujoco>
diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/launch/agent_bridge.launch.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/launch/agent_bridge.launch.xml
index b2aa7543..8eeec7d9 100644
--- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/launch/agent_bridge.launch.xml
+++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/launch/agent_bridge.launch.xml
@@ -1,6 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
 </launch>
diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/apriltag_pick_object.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/apriltag_pick_object.xml
index 3b1b5c6e..98140d18 100644
--- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/apriltag_pick_object.xml
+++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/apriltag_pick_object.xml
@@ -1,57 +1,27 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
   <!--//////////-->
-  <BehaviorTree
-    ID="Pick April Tag Labeled Object"
-    _description="Picks up an object that has an AprilTag marker."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Pick April Tag Labeled Object" _description="Picks up an object that has an AprilTag marker." _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
       <Control ID="Sequence" name="Setup">
-        <SubTree ID="Open Gripper" />
+        <SubTree ID="Open Gripper"/>
       </Control>
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="apriltag_detection_config.yaml"
-        parameters="{parameters}"
-      />
-      <Action
-        ID="GetCameraInfo"
-        topic_name="/wrist_camera/camera_info"
-        message_out="{camera_info}"
-      />
-      <Action
-        ID="GetImage"
-        topic_name="/wrist_camera/color"
-        message_out="{image}"
-      />
-      <Action
-        ID="DetectAprilTags"
-        image="{image}"
-        camera_info="{camera_info}"
-        parameters="{parameters}"
-        detections="{detections}"
-      />
-      <Action ID="GetDetectionPose" />
-      <Action ID="TransformPoseFrame" input_pose="{detection_pose}" />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{initial_poses}"
-        input="{output_pose}"
-      />
-      <SubTree
-        ID="Pick first object in vector"
-        _collapsed="true"
-        pose_vector="{initial_poses}"
-      />
+      <Action ID="LoadObjectiveParameters" config_file_name="apriltag_detection_config.yaml" parameters="{parameters}"/>
+      <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" message_out="{camera_info}"/>
+      <Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}"/>
+      <Action ID="DetectAprilTags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/>
+      <Action ID="GetDetectionPose"/>
+      <Action ID="TransformPoseFrame" input_pose="{detection_pose}"/>
+      <Action ID="AddPoseStampedToVector" vector="{initial_poses}" input="{output_pose}"/>
+      <SubTree ID="Pick first object in vector" _collapsed="true" pose_vector="{initial_poses}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Pick April Tag Labeled Object">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/pick_first_object_in_vector.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/pick_first_object_in_vector.xml
index 7d734095..c486ee6f 100644
--- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/pick_first_object_in_vector.xml
+++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/pick_first_object_in_vector.xml
@@ -1,47 +1,24 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick first object in vector">
   <!--//////////-->
-  <BehaviorTree
-    ID="Pick first object in vector"
-    _description="Pick the first object in a vector"
-  >
+  <BehaviorTree ID="Pick first object in vector" _description="Pick the first object in a vector">
     <Decorator ID="ForEach" vector_in="{pose_vector}" out="{grasp_pose}">
       <Control ID="Sequence">
-        <Action
-          ID="LoadObjectiveParameters"
-          config_file_name="pick_object_config.yaml"
-        />
-        <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}" />
+        <Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml"/>
+        <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>
     </Decorator>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Pick April Tag Labeled Object">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/pick_object.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/pick_object.xml
index cbb9a245..785ac9e7 100644
--- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/pick_object.xml
+++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/pick_object.xml
@@ -1,65 +1,30 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick object">
   <!--//////////-->
   <BehaviorTree ID="Pick object" _description="" _favorite="false">
     <Control ID="Sequence" name="root">
-      <SubTree ID="Open Gripper" />
+      <SubTree ID="Open Gripper"/>
       <Control ID="Sequence">
-        <Action
-          ID="GetPointsFromUser"
-          view_name="/wrist_camera/color"
-          point_names="grasp_pose"
-          point_prompts="Select the object you wish to pick"
-          pixel_coords="{pixel_coords}"
-        />
-        <Action
-          ID="GetPointCloud"
-          topic_name="/wrist_camera/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="GetPointsFromUser" view_name="/wrist_camera/color" point_names="grasp_pose" point_prompts="Select the object you wish to pick" pixel_coords="{pixel_coords}"/>
+        <Action ID="GetPointCloud" topic_name="/wrist_camera/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}"/>
         <!--Flip the direction of the grasp pose-->
         <Decorator ID="ForEach" vector_in="{output_poses}" out="{flipped_pose}">
-          <Action
-            ID="TransformPose"
-            output_pose="{corrected_pose}"
-            translation_xyz="0;0;0"
-            input_pose="{flipped_pose}"
-            quaternion_xyzw="0;1;0;0"
-          />
+          <Action ID="TransformPose" output_pose="{corrected_pose}" translation_xyz="0;0;0" input_pose="{flipped_pose}" quaternion_xyzw="0;1;0;0"/>
         </Decorator>
         <!--Create a vector of poses to match the input of AdjustPoseWithIMarker-->
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{corrected_pose}"
-          vector="{corrected_poses}"
-        />
-        <Action
-          ID="AdjustPoseWithIMarker"
-          prompts="Adjust IMarker to desired pose"
-          initial_poses="{corrected_poses}"
-          adjusted_poses="{adjusted_poses}"
-        />
+        <Action ID="AddPoseStampedToVector" input="{corrected_pose}" vector="{corrected_poses}"/>
+        <Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{corrected_poses}" adjusted_poses="{adjusted_poses}"/>
       </Control>
       <!--Use the ForEach decorator to get a pose from our vector of poses-->
-      <SubTree
-        ID="Pick first object in vector"
-        pose_vector="{adjusted_poses}"
-      />
+      <SubTree ID="Pick first object in vector" pose_vector="{adjusted_poses}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Pick object">
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
-        <Metadata runnable="false" />
+        <Metadata subcategory="Motion - Execute"/>
+        <Metadata runnable="false"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/pick_up_cube.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/pick_up_cube.xml
index 57051026..1d0fb2f2 100644
--- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/pick_up_cube.xml
+++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/pick_up_cube.xml
@@ -1,17 +1,17 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick up Cube">
   <!--//////////-->
   <BehaviorTree ID="Pick up Cube" _description="" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
-      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
-      <SubTree ID="Pick object" _collapsed="false" />
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true"/>
+      <SubTree ID="Pick object" _collapsed="false"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Pick up Cube">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/take_snap.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/take_snap.xml
index 89f2873c..f2a4953e 100644
--- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/take_snap.xml
+++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/take_snap.xml
@@ -1,22 +1,18 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Take scene camera snapshot">
   <!--//////////-->
-  <BehaviorTree
-    ID="Take scene camera snapshot"
-    _description=""
-    _favorite="true"
-  >
+  <BehaviorTree ID="Take scene camera snapshot" _description="" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Clear out old snapshot data -->
-      <Action ID="ClearSnapshot" />
-      <Action ID="GetPointCloud" topic_name="/scene_camera/points" />
-      <Action ID="SendPointCloudToUI" />
+      <Action ID="ClearSnapshot"/>
+      <Action ID="GetPointCloud" topic_name="/scene_camera/points"/>
+      <Action ID="SendPointCloudToUI"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Take scene camera snapshot">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/wrist_snap.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/wrist_snap.xml
index 324be61b..7c01ea60 100644
--- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/wrist_snap.xml
+++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/wrist_snap.xml
@@ -1,21 +1,17 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Take wrist camera snapshot">
   <!--//////////-->
-  <BehaviorTree
-    ID="Take wrist camera snapshot"
-    _description=""
-    _favorite="true"
-  >
+  <BehaviorTree ID="Take wrist camera snapshot" _description="" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Do not clear snapshots first, so we can take multiple snapshots from different angles -->
-      <Action ID="GetPointCloud" topic_name="/wrist_camera/points" />
-      <Action ID="SendPointCloudToUI" />
+      <Action ID="GetPointCloud" topic_name="/wrist_camera/points"/>
+      <Action ID="SendPointCloudToUI"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Take wrist camera snapshot">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/package.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/package.xml
index ce3395cb..f2050999 100644
--- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/package.xml
+++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>picknik_ur_mobile_config</name>
   <version>6.5.0</version>
diff --git a/src/moveit_pro_ur_configs/mock_sim/launch/agent_bridge.launch.xml b/src/moveit_pro_ur_configs/mock_sim/launch/agent_bridge.launch.xml
index b2aa7543..8eeec7d9 100644
--- a/src/moveit_pro_ur_configs/mock_sim/launch/agent_bridge.launch.xml
+++ b/src/moveit_pro_ur_configs/mock_sim/launch/agent_bridge.launch.xml
@@ -1,6 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
 </launch>
diff --git a/src/moveit_pro_ur_configs/mock_sim/objectives/3_waypoint_pick_and_place.xml b/src/moveit_pro_ur_configs/mock_sim/objectives/3_waypoint_pick_and_place.xml
index f5e855ce..f81b2602 100644
--- a/src/moveit_pro_ur_configs/mock_sim/objectives/3_waypoint_pick_and_place.xml
+++ b/src/moveit_pro_ur_configs/mock_sim/objectives/3_waypoint_pick_and_place.xml
@@ -1,102 +1,33 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="3 Waypoints Pick and Place">
   <!--//////////-->
-  <BehaviorTree
-    ID="3 Waypoints Pick and Place"
-    _description="Basic example of repeatedly grabbing a small (invisible) object, placing it at desired destination, and then moving back to a home position"
-    _favorite="true"
-  >
+  <BehaviorTree ID="3 Waypoints Pick and Place" _description="Basic example of repeatedly grabbing a small (invisible) object, placing it at desired destination, and then moving back to a home position" _favorite="true">
     <Control ID="Sequence">
       <!--Reset the planning scene to ensure no old collision objects are hanging around-->
-      <Action ID="ClearSnapshot" />
+      <Action ID="ClearSnapshot"/>
       <!--Setup the environment to its "home" configuration - move to center and open gripper-->
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Look at Machine"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0"
-      />
+      <SubTree ID="Move to Waypoint" waypoint_name="Look at Machine" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
       <!--Run pick and place on loop-->
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
           <!--Pick object from left, put it down at the CNC machine, and go back to center pose-->
           <Control ID="Sequence">
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Grasp Left"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            />
-            <Action
-              ID="MoveGripperAction"
-              gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-              position="0.7929"
-            />
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Look at Machine"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            />
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Grasp Machine"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            />
-            <Action
-              ID="MoveGripperAction"
-              gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-              position="0"
-            />
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Look at Machine"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            />
+            <SubTree ID="Move to Waypoint" waypoint_name="Grasp Left" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
+            <SubTree ID="Move to Waypoint" waypoint_name="Look at Machine" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <SubTree ID="Move to Waypoint" waypoint_name="Grasp Machine" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
+            <SubTree ID="Move to Waypoint" waypoint_name="Look at Machine" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
           </Control>
           <!--Pick object from the CNC machine, put it down on the right, and go back to center pose-->
           <Control ID="Sequence">
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Grasp Machine"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            />
-            <Action
-              ID="MoveGripperAction"
-              gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-              position="0.7929"
-            />
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Look at Machine"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            />
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Grasp Left"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            />
-            <Action
-              ID="MoveGripperAction"
-              gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-              position="0"
-            />
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Look at Machine"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            />
+            <SubTree ID="Move to Waypoint" waypoint_name="Grasp Machine" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
+            <SubTree ID="Move to Waypoint" waypoint_name="Look at Machine" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <SubTree ID="Move to Waypoint" waypoint_name="Grasp Left" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
+            <SubTree ID="Move to Waypoint" waypoint_name="Look at Machine" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
           </Control>
         </Control>
       </Decorator>
@@ -105,7 +36,7 @@
   <TreeNodesModel>
     <SubTree ID="3 Waypoint Pick and Place">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/mock_sim/objectives/constrained_pick_place.xml b/src/moveit_pro_ur_configs/mock_sim/objectives/constrained_pick_place.xml
index 61fbc76b..3aa7306e 100644
--- a/src/moveit_pro_ur_configs/mock_sim/objectives/constrained_pick_place.xml
+++ b/src/moveit_pro_ur_configs/mock_sim/objectives/constrained_pick_place.xml
@@ -1,42 +1,25 @@
-<?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">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Constrained Pick and Place"
-    _description="More advanced example of picking and placing an (invisible) object with orientation constraints enforced on the gripper's motion"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Constrained Pick and Place" _description="More advanced example of picking and placing an (invisible) object with orientation constraints enforced on the gripper's motion" _favorite="true">
     <Decorator ID="KeepRunningUntilFailure">
       <Control ID="Sequence">
         <!--Move to pick location-->
-        <SubTree ID="Open Gripper" />
+        <SubTree ID="Open Gripper"/>
         <!--Note: Sampling based planners can be non-deterministic. The retry decorator improves the likelihood of success-->
-        <SubTree
-          ID="Move to Waypoint"
-          waypoint_name="Grasp Right"
-          joint_group_name="manipulator"
-          controller_names="/joint_trajectory_controller"
-        />
+        <SubTree ID="Move to Waypoint" waypoint_name="Grasp Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
         <!--Pick-->
-        <SubTree ID="Close Gripper" />
+        <SubTree ID="Close Gripper"/>
         <!--Move to place (drop) location-->
-        <SubTree
-          ID="Move to Waypoint"
-          controller_names="/joint_trajectory_controller"
-          joint_group_name="manipulator"
-          waypoint_name="Grasp Left"
-          link_padding="0.0"
-          keep_orientation="true"
-          keep_orientation_tolerance="0.2"
-        />
+        <SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller" joint_group_name="manipulator" waypoint_name="Grasp Left" link_padding="0.0" keep_orientation="true" keep_orientation_tolerance="0.2"/>
       </Control>
     </Decorator>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Constrained Pick and Place">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/mock_sim/objectives/cycle_between_waypoints.xml b/src/moveit_pro_ur_configs/mock_sim/objectives/cycle_between_waypoints.xml
index 10c4359e..0a3f12f5 100644
--- a/src/moveit_pro_ur_configs/mock_sim/objectives/cycle_between_waypoints.xml
+++ b/src/moveit_pro_ur_configs/mock_sim/objectives/cycle_between_waypoints.xml
@@ -1,31 +1,17 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Cycle Between Waypoints">
-  <BehaviorTree
-    ID="Cycle Between Waypoints"
-    _description="Example of repeatedly moving between two saved poses."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Cycle Between Waypoints" _description="Example of repeatedly moving between two saved poses." _favorite="true">
     <Decorator ID="KeepRunningUntilFailure">
       <Control ID="Sequence">
-        <SubTree
-          ID="Move to Waypoint"
-          waypoint_name="Grasp Left"
-          joint_group_name="manipulator"
-          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        />
-        <SubTree
-          ID="Move to Waypoint"
-          waypoint_name="Grasp Right"
-          joint_group_name="manipulator"
-          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        />
+        <SubTree ID="Move to Waypoint" waypoint_name="Grasp Left" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+        <SubTree ID="Move to Waypoint" waypoint_name="Grasp Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
       </Control>
     </Decorator>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Cycle Between Waypoints">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/mock_sim/objectives/joint_diagnostic.xml b/src/moveit_pro_ur_configs/mock_sim/objectives/joint_diagnostic.xml
index fc631d81..280dba58 100644
--- a/src/moveit_pro_ur_configs/mock_sim/objectives/joint_diagnostic.xml
+++ b/src/moveit_pro_ur_configs/mock_sim/objectives/joint_diagnostic.xml
@@ -1,55 +1,15 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Joint Diagnostic">
   <!--//////////-->
-  <BehaviorTree
-    ID="Joint Diagnostic"
-    _description="Example of cycling through the min and max limits of a single joint, for hardware testing"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Joint Diagnostic" _description="Example of cycling through the min and max limits of a single joint, for hardware testing" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <!--Setup by moving home-->
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Home"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-        keep_orientation="false"
-        keep_orientation_link_names="grasp_link"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.01"
-        velocity_scale_factor="1.0"
-      />
+      <SubTree ID="Move to Waypoint" waypoint_name="Home" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0"/>
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
           <!--If you use two waypoints that have identical joint values, except for one joint, you can use this Objective as a way to test a single joint repeatedly. This is useful for things like hardware diagnostics and hardware bringup.-->
-          <SubTree
-            ID="Move to Waypoint"
-            waypoint_name="Wrist 2 Max"
-            joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            acceleration_scale_factor="1.0"
-            controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-            keep_orientation="false"
-            keep_orientation_link_names="grasp_link"
-            keep_orientation_tolerance="0.05"
-            link_padding="0.0"
-            velocity_scale_factor="1.0"
-          />
-          <SubTree
-            ID="Move to Waypoint"
-            waypoint_name="Wrist 2 Min"
-            joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            acceleration_scale_factor="1.0"
-            controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
-            keep_orientation="false"
-            keep_orientation_link_names="grasp_link"
-            keep_orientation_tolerance="0.05"
-            link_padding="0.0"
-            velocity_scale_factor="1.0"
-          />
+          <SubTree ID="Move to Waypoint" waypoint_name="Wrist 2 Max" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.05" link_padding="0.0" velocity_scale_factor="1.0"/>
+          <SubTree ID="Move to Waypoint" waypoint_name="Wrist 2 Min" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" keep_orientation="false" keep_orientation_link_names="grasp_link" keep_orientation_tolerance="0.05" link_padding="0.0" velocity_scale_factor="1.0"/>
         </Control>
       </Decorator>
     </Control>
@@ -57,8 +17,8 @@
   <TreeNodesModel>
     <SubTree ID="Joint Diagnostic">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Basic Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/mock_sim/objectives/move_along_path.xml b/src/moveit_pro_ur_configs/mock_sim/objectives/move_along_path.xml
index 04a680f4..02098264 100644
--- a/src/moveit_pro_ur_configs/mock_sim/objectives/move_along_path.xml
+++ b/src/moveit_pro_ur_configs/mock_sim/objectives/move_along_path.xml
@@ -1,119 +1,31 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move Along Path">
   <!--//////////-->
-  <BehaviorTree
-    ID="Move Along Path"
-    _description="Example of using Cartesian control to draw a rectangle in the air."
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Move Along Path" _description="Example of using Cartesian control to draw a rectangle in the air." _favorite="true" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <!--First manually create an example Cartesian reference trajectory to then feed into the planner-->
       <Control ID="Sequence">
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="-0.4;0.4;-0.0"
-          orientation_xyzw="0.0;1.0;0.0;0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.4;0.4;-0.0"
-          orientation_xyzw="0.0;1.0;0.0;0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="0.4;0.8;0.0"
-          orientation_xyzw="0.0;1.0;0.0;0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="-0.4;0.8;0.0"
-          orientation_xyzw="0.0;1.0;0.0;0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
-        <Action
-          ID="CreateStampedPose"
-          reference_frame="base_link"
-          position_xyz="-0.4;0.4;-0.0"
-          orientation_xyzw="0.0;1.0;0.0;0.0"
-          stamped_pose="{stamped_pose}"
-        />
-        <Action
-          ID="AddPoseStampedToVector"
-          input="{stamped_pose}"
-          vector="{pose_stamped_vector}"
-        />
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="-0.4;0.4;-0.0" orientation_xyzw="0.0;1.0;0.0;0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.4;0.4;-0.0" orientation_xyzw="0.0;1.0;0.0;0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="0.4;0.8;0.0" orientation_xyzw="0.0;1.0;0.0;0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="-0.4;0.8;0.0" orientation_xyzw="0.0;1.0;0.0;0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
+        <Action ID="CreateStampedPose" reference_frame="base_link" position_xyz="-0.4;0.4;-0.0" orientation_xyzw="0.0;1.0;0.0;0.0" stamped_pose="{stamped_pose}"/>
+        <Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_stamped_vector}"/>
       </Control>
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
           <!--Move to starting position, before planning and executing Cartesian path-->
-          <SubTree
-            ID="Move to Waypoint"
-            waypoint_name="Look at Right Table"
-            joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller"
-          />
+          <SubTree ID="Move to Waypoint" waypoint_name="Look at Right Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller"/>
           <!--Plan and validate the trajectory-->
-          <Action
-            ID="PlanCartesianPath"
-            path="{pose_stamped_vector}"
-            planning_group_name="manipulator"
-            tip_link="grasp_link"
-            tip_offset="0.0;0.0;0.0;0.0;0.0;0.0"
-            position_only="false"
-            blending_radius="0.02"
-            velocity_scale_factor="1.0"
-            acceleration_scale_factor="1.0"
-            trajectory_sampling_rate="100"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-            ik_joint_space_density="0.05000"
-            ik_cartesian_space_density="0.01000"
-          />
-          <Action
-            ID="GetCurrentPlanningScene"
-            planning_scene_msg="{planning_scene_msg}"
-          />
-          <Action
-            ID="ValidateTrajectory"
-            planning_scene_msg="{planning_scene_msg}"
-            planning_group_name="manipulator"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-            joint_space_step="0.05"
-            cartesian_space_step="0.02"
-          />
+          <Action ID="PlanCartesianPath" path="{pose_stamped_vector}" planning_group_name="manipulator" tip_link="grasp_link" tip_offset="0.0;0.0;0.0;0.0;0.0;0.0" position_only="false" blending_radius="0.02" velocity_scale_factor="1.0" acceleration_scale_factor="1.0" trajectory_sampling_rate="100" joint_trajectory_msg="{joint_trajectory_msg}" ik_joint_space_density="0.05000" ik_cartesian_space_density="0.01000"/>
+          <Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene_msg}"/>
+          <Action ID="ValidateTrajectory" planning_scene_msg="{planning_scene_msg}" planning_group_name="manipulator" joint_trajectory_msg="{joint_trajectory_msg}" joint_space_step="0.05" cartesian_space_step="0.02"/>
           <!--Finally, execute the trajectory on the actual robot-->
-          <Action
-            ID="ExecuteFollowJointTrajectory"
-            execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
-            joint_trajectory_msg="{joint_trajectory_msg}"
-          />
+          <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" joint_trajectory_msg="{joint_trajectory_msg}"/>
         </Control>
       </Decorator>
     </Control>
@@ -121,7 +33,7 @@
   <TreeNodesModel>
     <SubTree ID="Move Along Path">
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/mock_sim/objectives/rrt_connect_test.xml b/src/moveit_pro_ur_configs/mock_sim/objectives/rrt_connect_test.xml
index 92c362f6..fd565090 100644
--- a/src/moveit_pro_ur_configs/mock_sim/objectives/rrt_connect_test.xml
+++ b/src/moveit_pro_ur_configs/mock_sim/objectives/rrt_connect_test.xml
@@ -1,35 +1,17 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="RRTConnect Test">
   <!--//////////-->
-  <BehaviorTree
-    ID="RRTConnect Test"
-    _description="A test to move between waypoints using RRTConnect"
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="RRTConnect Test" _description="A test to move between waypoints using RRTConnect" _favorite="true" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="ActivateControllers"
-        controller_names="joint_trajectory_controller"
-      />
+      <Action ID="ActivateControllers" controller_names="joint_trajectory_controller"/>
       <Decorator ID="Repeat" num_cycles="10">
         <Control ID="Sequence">
-          <Action
-            ID="RetrieveWaypoint"
-            waypoint_name="Grasp Left"
-            waypoint_joint_state="{grasp_left}"
-            joint_group_name="manipulator"
-          />
-          <Action ID="PlanToJointGoal" joint_goal="{grasp_left}" />
-          <Action ID="ExecuteFollowJointTrajectory" />
-          <Action
-            ID="RetrieveWaypoint"
-            waypoint_joint_state="{grasp_right}"
-            joint_group_name="manipulator"
-            waypoint_name="Grasp Right"
-          />
-          <Action ID="PlanToJointGoal" joint_goal="{grasp_right}" />
-          <Action ID="ExecuteFollowJointTrajectory" />
+          <Action ID="RetrieveWaypoint" waypoint_name="Grasp Left" waypoint_joint_state="{grasp_left}" joint_group_name="manipulator"/>
+          <Action ID="PlanToJointGoal" joint_goal="{grasp_left}"/>
+          <Action ID="ExecuteFollowJointTrajectory"/>
+          <Action ID="RetrieveWaypoint" waypoint_joint_state="{grasp_right}" joint_group_name="manipulator" waypoint_name="Grasp Right"/>
+          <Action ID="PlanToJointGoal" joint_goal="{grasp_right}"/>
+          <Action ID="ExecuteFollowJointTrajectory"/>
         </Control>
       </Decorator>
     </Control>
@@ -37,7 +19,7 @@
   <TreeNodesModel>
     <SubTree ID="RRTConnect Test">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/mock_sim/package.xml b/src/moveit_pro_ur_configs/mock_sim/package.xml
index f1b32131..df393418 100644
--- a/src/moveit_pro_ur_configs/mock_sim/package.xml
+++ b/src/moveit_pro_ur_configs/mock_sim/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>mock_sim</name>
   <version>6.5.0</version>
diff --git a/src/moveit_pro_ur_configs/moveit_studio_ur_pstop_manager/package.xml b/src/moveit_pro_ur_configs/moveit_studio_ur_pstop_manager/package.xml
index 47a31247..f4a21ca0 100644
--- a/src/moveit_pro_ur_configs/moveit_studio_ur_pstop_manager/package.xml
+++ b/src/moveit_pro_ur_configs/moveit_studio_ur_pstop_manager/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>moveit_studio_ur_pstop_manager</name>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/launch/agent_bridge.launch.xml b/src/moveit_pro_ur_configs/multi_arm_sim/launch/agent_bridge.launch.xml
index b2aa7543..8eeec7d9 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/launch/agent_bridge.launch.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/launch/agent_bridge.launch.xml
@@ -1,6 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
 </launch>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/close_left_gripper.xml b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/close_left_gripper.xml
index e59ca6c4..f933e6c6 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/close_left_gripper.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/close_left_gripper.xml
@@ -1,21 +1,14 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Close Second Gripper">
-  <BehaviorTree
-    ID="Close Second Gripper"
-    _description="Close the second gripper"
-  >
+  <BehaviorTree ID="Close Second Gripper" _description="Close the second gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/second_robotiq_gripper_controller/gripper_cmd"
-        position="0.7929"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/second_robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Close Second Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/close_right_gripper.xml b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/close_right_gripper.xml
index 5f6fe19d..bf7f789f 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/close_right_gripper.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/close_right_gripper.xml
@@ -1,18 +1,14 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Close First Gripper">
   <BehaviorTree ID="Close First Gripper" _description="Close the first gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/first_robotiq_gripper_controller/gripper_cmd"
-        position="0.7929"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/first_robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Close First Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/interpolate_to_joint_state.xml b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/interpolate_to_joint_state.xml
index 67b02a66..d23be22d 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/interpolate_to_joint_state.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/interpolate_to_joint_state.xml
@@ -1,40 +1,19 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Interpolate to Joint State">
-  <BehaviorTree
-    ID="Interpolate to Joint State"
-    _description="Move to a specified joint state using joint interpolation"
-  >
+  <BehaviorTree ID="Interpolate to Joint State" _description="Move to a specified joint state using joint interpolation">
     <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="interpolate_to_joint_state"
-        controller_names="/multi_arm_joint_trajectory_controller"
-        task="{interpolate_to_waypoint_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}" />
-      <Action
-        ID="SetupMTCInterpolateToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCInterpolateToJointState_First"
-        planning_group_name="multi_arm_manipulator"
-        task="{interpolate_to_waypoint_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{interpolate_to_waypoint_solution}"
-        task="{interpolate_to_waypoint_task}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{interpolate_to_waypoint_solution}"
-      />
+      <Action ID="InitializeMTCTask" task_id="interpolate_to_joint_state" controller_names="/multi_arm_joint_trajectory_controller" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="SetupMTCInterpolateToJointState" joint_state="{target_joint_state}" name="SetupMTCInterpolateToJointState_First" planning_group_name="multi_arm_manipulator" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="PlanMTCTask" solution="{interpolate_to_waypoint_solution}" task="{interpolate_to_waypoint_task}"/>
+      <Action ID="ExecuteMTCTask" solution="{interpolate_to_waypoint_solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Interpolate to Joint State">
-      <input_port name="target_joint_state" default="{target_joint_state}" />
+      <input_port name="target_joint_state" default="{target_joint_state}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_to_joint_state.xml b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_to_joint_state.xml
index d434f523..9e1402d3 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_to_joint_state.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_to_joint_state.xml
@@ -1,47 +1,25 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
-  <BehaviorTree
-    ID="Move to Joint State"
-    _description="Plan and execute motion to a set of joint angles specified in a ROS message type."
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Move to Joint State" _description="Plan and execute motion to a set of joint angles specified in a ROS message type." _subtreeOnly="true">
     <Control ID="Sequence">
-      <Action ID="ActivateControllers" controller_names="{controller_names}" />
-      <Action
-        ID="PlanToJointGoal"
-        joint_goal="{target_joint_state}"
-        keep_orientation_tolerance="{keep_orientation_tolerance}"
-        link_padding="{link_padding}"
-        keep_orientation="{keep_orientation}"
-        acceleration_scale_factor="{acceleration_scale_factor}"
-        velocity_scale_factor="{velocity_scale_factor}"
-        planning_group_name="{joint_group_name}"
-      />
-      <Action
-        ID="ExecuteFollowJointTrajectory"
-        execute_follow_joint_trajectory_action_name="{controller_action_server}"
-      />
+      <Action ID="ActivateControllers" controller_names="{controller_names}"/>
+      <Action ID="PlanToJointGoal" joint_goal="{target_joint_state}" keep_orientation_tolerance="{keep_orientation_tolerance}" link_padding="{link_padding}" keep_orientation="{keep_orientation}" acceleration_scale_factor="{acceleration_scale_factor}" velocity_scale_factor="{velocity_scale_factor}" planning_group_name="{joint_group_name}"/>
+      <Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="{controller_action_server}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Joint State">
-      <input_port name="target_joint_state" default="{target_joint_state}" />
-      <input_port name="joint_group_name" default="manipulator" />
-      <input_port name="link_padding" default="0.01" />
-      <input_port name="keep_orientation_tolerance" default="0.05" />
-      <input_port name="keep_orientation" default="false" />
-      <input_port name="velocity_scale_factor" default="1.0" />
-      <input_port name="acceleration_scale_factor" default="1.0" />
-      <input_port
-        name="controller_names"
-        default="joint_trajectory_controller"
-      />
-      <input_port
-        name="controller_action_server"
-        default="/multi_arm_joint_trajectory_controller/follow_joint_trajectory"
-      />
+      <input_port name="target_joint_state" default="{target_joint_state}"/>
+      <input_port name="joint_group_name" default="manipulator"/>
+      <input_port name="link_padding" default="0.01"/>
+      <input_port name="keep_orientation_tolerance" default="0.05"/>
+      <input_port name="keep_orientation" default="false"/>
+      <input_port name="velocity_scale_factor" default="1.0"/>
+      <input_port name="acceleration_scale_factor" default="1.0"/>
+      <input_port name="controller_names" default="joint_trajectory_controller"/>
+      <input_port name="controller_action_server" default="/multi_arm_joint_trajectory_controller/follow_joint_trajectory"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_to_pose.xml b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_to_pose.xml
index 47af8a24..ae0ddfb7 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_to_pose.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_to_pose.xml
@@ -1,41 +1,20 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Pose">
-  <BehaviorTree
-    ID="Move to Pose"
-    _description="Plan and execute motion to a Cartesian coordinate pose in a ROS message type, using inverse kinematics."
-  >
+  <BehaviorTree ID="Move to Pose" _description="Plan and execute motion to a Cartesian coordinate pose in a ROS message type, using inverse kinematics.">
     <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_pose"
-        controller_names="/multi_arm_joint_trajectory_controller"
-        task="{move_to_pose_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
-      <Action
-        ID="SetupMTCPlanToPose"
-        ik_frame="first_grasp_link"
-        planning_group_name="first_manipulator"
-        target_pose="{target_pose}"
-        task="{move_to_pose_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_pose_solution}"
-        task="{move_to_pose_task}"
-      />
-      <SubTree
-        ID="Wait for Trajectory Approval if User Available"
-        solution="{move_to_pose_solution}"
-      />
-      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
+      <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/multi_arm_joint_trajectory_controller" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCPlanToPose" ik_frame="first_grasp_link" planning_group_name="first_manipulator" target_pose="{target_pose}" task="{move_to_pose_task}"/>
+      <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+      <SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Pose">
-      <input_port name="target_pose" default="{target_pose}" />
+      <input_port name="target_pose" default="{target_pose}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_to_waypoint.xml b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_to_waypoint.xml
index b031e9b0..79afb76d 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_to_waypoint.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_to_waypoint.xml
@@ -1,51 +1,24 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
-  <BehaviorTree
-    ID="Move to Waypoint"
-    _description="Plan and execute motion to a MoveIt Pro-created named Waypoint, which is saved in joint space to the file system."
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Move to Waypoint" _description="Plan and execute motion to a MoveIt Pro-created named Waypoint, which is saved in joint space to the file system." _subtreeOnly="true">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="RetrieveWaypoint"
-        waypoint_joint_state="{target_joint_state}"
-        waypoint_name="{waypoint_name}"
-        joint_group_name="{joint_group_name}"
-      />
-      <SubTree
-        ID="Move to Joint State"
-        _collapsed="true"
-        velocity_scale_factor="{velocity_scale_factor}"
-        acceleration_scale_factor="{acceleration_scale_factor}"
-        link_padding="{link_padding}"
-        keep_orientation_tolerance="{keep_orientation_tolerance}"
-        keep_orientation="{keep_orientation}"
-        target_joint_state="{target_joint_state}"
-        controller_names="{controller_names}"
-        joint_group_name="{joint_group_name}"
-        controller_action_server="{controller_action_server}"
-      />
+      <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}" joint_group_name="{joint_group_name}"/>
+      <SubTree ID="Move to Joint State" _collapsed="true" velocity_scale_factor="{velocity_scale_factor}" acceleration_scale_factor="{acceleration_scale_factor}" link_padding="{link_padding}" keep_orientation_tolerance="{keep_orientation_tolerance}" keep_orientation="{keep_orientation}" target_joint_state="{target_joint_state}" controller_names="{controller_names}" joint_group_name="{joint_group_name}" controller_action_server="{controller_action_server}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Waypoint">
-      <input_port name="waypoint_name" default="" />
-      <input_port name="joint_group_name" default="manipulator" />
-      <input_port name="link_padding" default="0.01" />
-      <input_port name="keep_orientation_tolerance" default="0.05" />
-      <input_port name="keep_orientation" default="false" />
-      <input_port name="velocity_scale_factor" default="1.0" />
-      <input_port name="acceleration_scale_factor" default="1.0" />
-      <input_port
-        name="controller_names"
-        default="/multi_arm_joint_trajectory_controller"
-      />
-      <input_port
-        name="controller_action_server"
-        default="/multi_arm_joint_trajectory_controller/follow_joint_trajectory"
-      />
+      <input_port name="waypoint_name" default=""/>
+      <input_port name="joint_group_name" default="manipulator"/>
+      <input_port name="link_padding" default="0.01"/>
+      <input_port name="keep_orientation_tolerance" default="0.05"/>
+      <input_port name="keep_orientation" default="false"/>
+      <input_port name="velocity_scale_factor" default="1.0"/>
+      <input_port name="acceleration_scale_factor" default="1.0"/>
+      <input_port name="controller_names" default="/multi_arm_joint_trajectory_controller"/>
+      <input_port name="controller_action_server" default="/multi_arm_joint_trajectory_controller/follow_joint_trajectory"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_two_arms.xml b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_two_arms.xml
index 77ff3bee..005870df 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_two_arms.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_two_arms.xml
@@ -1,49 +1,18 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Cycle Between Poses">
   <!--//////////-->
-  <BehaviorTree
-    ID="Cycle Between Poses"
-    _description="Simple example objective for multi arm motions. Two arms are moved to different waypoints."
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Cycle Between Poses" _description="Simple example objective for multi arm motions. Two arms are moved to different waypoints." _favorite="true" _hardcoded="false">
     <Decorator ID="KeepRunningUntilFailure">
       <Control ID="Sequence">
-        <SubTree
-          ID="Move to Waypoint"
-          _collapsed="true"
-          joint_group_name="multi_arm_manipulator"
-          controller_names="/multi_arm_joint_trajectory_controller"
-          velocity_scale_factor="1.0"
-          acceleration_scale_factor="1.0"
-          link_padding="0.0"
-          keep_orientation_tolerance="0.05"
-          keep_orientation="false"
-          waypoint_name="Home"
-          target_joint_state="{target_joint_state}"
-          controller_action_server="/multi_arm_joint_trajectory_controller/follow_joint_trajectory"
-        />
-        <SubTree
-          ID="Move to Waypoint"
-          _collapsed="true"
-          joint_group_name="multi_arm_manipulator"
-          controller_names="/multi_arm_joint_trajectory_controller"
-          velocity_scale_factor="1.0"
-          acceleration_scale_factor="1.0"
-          link_padding="0.0"
-          keep_orientation_tolerance="0.05"
-          keep_orientation="false"
-          waypoint_name="Flip"
-          target_joint_state="{target_joint_state}"
-          controller_action_server="/multi_arm_joint_trajectory_controller/follow_joint_trajectory"
-        />
+        <SubTree ID="Move to Waypoint" _collapsed="true" joint_group_name="multi_arm_manipulator" controller_names="/multi_arm_joint_trajectory_controller" velocity_scale_factor="1.0" acceleration_scale_factor="1.0" link_padding="0.0" keep_orientation_tolerance="0.05" keep_orientation="false" waypoint_name="Home" target_joint_state="{target_joint_state}" controller_action_server="/multi_arm_joint_trajectory_controller/follow_joint_trajectory"/>
+        <SubTree ID="Move to Waypoint" _collapsed="true" joint_group_name="multi_arm_manipulator" controller_names="/multi_arm_joint_trajectory_controller" velocity_scale_factor="1.0" acceleration_scale_factor="1.0" link_padding="0.0" keep_orientation_tolerance="0.05" keep_orientation="false" waypoint_name="Flip" target_joint_state="{target_joint_state}" controller_action_server="/multi_arm_joint_trajectory_controller/follow_joint_trajectory"/>
       </Control>
     </Decorator>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Cycle Between Poses">
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/multi_tip_ik_example.xml b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/multi_tip_ik_example.xml
index d8972fbf..016c7fa2 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/multi_tip_ik_example.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/multi_tip_ik_example.xml
@@ -1,96 +1,29 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Multi-tip IK Example">
   <!--//////////-->
   <BehaviorTree ID="Multi-tip IK Example" _description="" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <!--Create the target poses into a vector-->
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="world"
-        stamped_pose="{target}"
-        position_xyz="-0.6;0.5;0.2"
-        orientation_xyzw="0.7071068;-0.7071068;0;0"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="target_pose_1"
-        marker_size="0.100000"
-        pose="{target}"
-      />
-      <Action
-        ID="TransformPose"
-        input_pose="{target}"
-        output_pose="{target_pose_1}"
-        quaternion_xyzw="0;0.7071068;0;0.7071068"
-        translation_xyz="-0.05;0;0"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="target_pose_1"
-        marker_size="0.100000"
-        pose="{target_pose_1}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{target_pose_1}"
-        vector="{target_poses}"
-      />
-      <Action
-        ID="TransformPose"
-        input_pose="{target}"
-        output_pose="{target_pose_2}"
-        quaternion_xyzw="0;0.7071068;0;-0.7071068"
-        translation_xyz="0.05;0;0"
-      />
-      <Action
-        ID="VisualizePose"
-        marker_lifetime="0.000000"
-        marker_name="target_pose_2"
-        marker_size="0.100000"
-        pose="{target_pose_2}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        input="{target_pose_2}"
-        vector="{target_poses}"
-      />
+      <Action ID="CreateStampedPose" reference_frame="world" stamped_pose="{target}" position_xyz="-0.6;0.5;0.2" orientation_xyzw="0.7071068;-0.7071068;0;0"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="target_pose_1" marker_size="0.100000" pose="{target}"/>
+      <Action ID="TransformPose" input_pose="{target}" output_pose="{target_pose_1}" quaternion_xyzw="0;0.7071068;0;0.7071068" translation_xyz="-0.05;0;0"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="target_pose_1" marker_size="0.100000" pose="{target_pose_1}"/>
+      <Action ID="AddPoseStampedToVector" input="{target_pose_1}" vector="{target_poses}"/>
+      <Action ID="TransformPose" input_pose="{target}" output_pose="{target_pose_2}" quaternion_xyzw="0;0.7071068;0;-0.7071068" translation_xyz="0.05;0;0"/>
+      <Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="target_pose_2" marker_size="0.100000" pose="{target_pose_2}"/>
+      <Action ID="AddPoseStampedToVector" input="{target_pose_2}" vector="{target_poses}"/>
       <!--Compute IK simultaneously for two tips-->
-      <Action
-        ID="GetCurrentPlanningScene"
-        planning_scene_msg="{planning_scene}"
-      />
-      <Action
-        ID="ComputeInverseKinematics"
-        ik_timeout_s="0.01"
-        planning_group_name="multi_arm_manipulator"
-        planning_scene_msg="{planning_scene}"
-        target_poses="{target_poses}"
-        tip_links="fourth_grasp_link;third_grasp_link"
-        solution="{solution}"
-        link_padding="0.01"
-      />
+      <Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene}"/>
+      <Action ID="ComputeInverseKinematics" ik_timeout_s="0.01" planning_group_name="multi_arm_manipulator" planning_scene_msg="{planning_scene}" target_poses="{target_poses}" tip_links="fourth_grasp_link;third_grasp_link" solution="{solution}" link_padding="0.01"/>
       <!--Move to the computed solution-->
-      <SubTree
-        ID="Move to Joint State"
-        _collapsed="false"
-        acceleration_scale_factor="1.0"
-        controller_action_server="/multi_arm_joint_trajectory_controller/follow_joint_trajectory"
-        controller_names="/multi_arm_joint_trajectory_controller"
-        joint_group_name="multi_arm_manipulator"
-        keep_orientation="false"
-        keep_orientation_tolerance="0.05"
-        link_padding="0.00"
-        target_joint_state="{solution}"
-        velocity_scale_factor="1.0"
-      />
+      <SubTree ID="Move to Joint State" _collapsed="false" acceleration_scale_factor="1.0" controller_action_server="/multi_arm_joint_trajectory_controller/follow_joint_trajectory" controller_names="/multi_arm_joint_trajectory_controller" joint_group_name="multi_arm_manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.00" target_joint_state="{solution}" velocity_scale_factor="1.0"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Multi-tip IK Example">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/open_left_gripper.xml b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/open_left_gripper.xml
index a5211d5e..a19f73b0 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/open_left_gripper.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/open_left_gripper.xml
@@ -1,19 +1,15 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Open Second Gripper">
   <!-- ////////// -->
   <BehaviorTree ID="Open Second Gripper" _description="Open the second gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/second_robotiq_gripper_controller/gripper_cmd"
-        position="0.0"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/second_robotiq_gripper_controller/gripper_cmd" position="0.0"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Open Second Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/open_right_gripper.xml b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/open_right_gripper.xml
index 0ffd476d..5f6f486a 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/open_right_gripper.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/open_right_gripper.xml
@@ -1,19 +1,15 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Open First Gripper">
   <!-- ////////// -->
   <BehaviorTree ID="Open First Gripper" _description="Open the first gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/first_robotiq_gripper_controller/gripper_cmd"
-        position="0.0"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/first_robotiq_gripper_controller/gripper_cmd" position="0.0"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Open First Gripper">
       <MetadataFields>
-        <Metadata subcategory="User Input" />
+        <Metadata subcategory="User Input"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/request_teleoperation.xml b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/request_teleoperation.xml
index 29732a53..5616f76b 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/request_teleoperation.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/request_teleoperation.xml
@@ -1,61 +1,32 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Request Teleoperation">
   <!--The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message.-->
-  <BehaviorTree
-    ID="Request Teleoperation"
-    _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective."
-    _favorite="false"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Request Teleoperation" _description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective." _favorite="false" _hardcoded="false">
     <Control ID="Sequence">
-      <Action ID="Script" code="teleop_mode := 0" />
+      <Action ID="Script" code="teleop_mode := 0"/>
       <Control ID="Parallel" success_count="1" failure_count="1">
-        <Action
-          ID="DoTeleoperateAction"
-          enable_user_interaction="{enable_user_interaction}"
-          user_interaction_prompt="{user_interaction_prompt}"
-          initial_teleop_mode="{initial_teleop_mode}"
-          current_teleop_mode="{teleop_mode}"
-        />
+        <Action ID="DoTeleoperateAction" enable_user_interaction="{enable_user_interaction}" user_interaction_prompt="{user_interaction_prompt}" initial_teleop_mode="{initial_teleop_mode}" current_teleop_mode="{teleop_mode}"/>
         <Decorator ID="KeepRunningUntilFailure">
           <Control ID="Sequence">
             <!--Closing and opening the gripper-->
             <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 7">
-              <SubTree ID="Close Second Gripper" _collapsed="true" />
+              <SubTree ID="Close Second Gripper" _collapsed="true"/>
             </Decorator>
             <Decorator ID="ForceSuccess" _skipIf="teleop_mode != 6">
-              <SubTree ID="Open Second Gripper" _collapsed="true" />
+              <SubTree ID="Open Second Gripper" _collapsed="true"/>
             </Decorator>
             <!--Joint sliders interpolate to joint state-->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 5">
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrieveJointStateParameter"
-                      timeout_sec="-1"
-                      joint_state="{target_joint_state}"
-                    />
-                    <SubTree
-                      ID="Interpolate to Joint State"
-                      _collapsed="false"
-                      target_joint_state="{target_joint_state}"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
+                    <SubTree ID="Interpolate to Joint State" _collapsed="false" target_joint_state="{target_joint_state}"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
@@ -65,31 +36,13 @@
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrievePoseParameter"
-                      timeout_sec="-1"
-                      pose="{target_pose}"
-                    />
-                    <SubTree
-                      ID="Move to Pose"
-                      _collapsed="false"
-                      target_pose="{target_pose}"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrievePoseParameter" timeout_sec="-1" pose="{target_pose}"/>
+                    <SubTree ID="Move to Pose" _collapsed="false" target_pose="{target_pose}"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
@@ -99,48 +52,24 @@
               <Control ID="Sequence">
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
-                    <Action
-                      ID="RetrieveJointStateParameter"
-                      timeout_sec="-1"
-                      joint_state="{target_joint_state}"
-                    />
-                    <SubTree
-                      ID="Move to Joint State"
-                      _collapsed="false"
-                      target_joint_state="{target_joint_state}"
-                    />
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
+                    <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
+                    <SubTree ID="Move to Joint State" _collapsed="false" target_joint_state="{target_joint_state}"/>
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
                   </Control>
                   <Control ID="Sequence">
-                    <Action
-                      ID="PublishEmpty"
-                      topic="/studio_ui/motion_ended"
-                      queue_size="1"
-                      use_best_effort="false"
-                    />
-                    <Action ID="AlwaysFailure" />
+                    <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
+                    <Action ID="AlwaysFailure"/>
                   </Control>
                 </Control>
               </Control>
             </Decorator>
             <!--Pose Jog-->
             <Control ID="Sequence" _while="teleop_mode == 2">
-              <Action
-                ID="TeleoperateTwist"
-                controller_name="servo_controller"
-              />
+              <Action ID="TeleoperateTwist" controller_name="servo_controller"/>
             </Control>
             <!--Joint Jog-->
             <Control ID="Sequence" _while="teleop_mode == 1">
-              <Action
-                ID="TeleoperateJointJog"
-                controller_name="servo_controller"
-              />
+              <Action ID="TeleoperateJointJog" controller_name="servo_controller"/>
             </Control>
           </Control>
         </Decorator>
@@ -150,7 +79,7 @@
   <TreeNodesModel>
     <SubTree ID="Request Teleoperation">
       <MetadataFields>
-        <Metadata subcategory="User Input" />
+        <Metadata subcategory="User Input"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/wait_for_trajectory_approval_if_user_available.xml b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/wait_for_trajectory_approval_if_user_available.xml
index 1b57c88b..263b00fa 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/objectives/wait_for_trajectory_approval_if_user_available.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/objectives/wait_for_trajectory_approval_if_user_available.xml
@@ -1,23 +1,17 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Wait for Trajectory Approval if User Available"
->
-  <BehaviorTree
-    ID="Wait for Trajectory Approval if User Available"
-    _description="Preview a shared pointer to an MTC Solution object via an input data port if a user is available."
-  >
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Wait for Trajectory Approval if User Available">
+  <BehaviorTree ID="Wait for Trajectory Approval if User Available" _description="Preview a shared pointer to an MTC Solution object via an input data port if a user is available.">
     <Control ID="Fallback" name="wait_for_approval_if_user_available">
       <Decorator ID="Inverter">
-        <Action ID="IsUserAvailable" />
+        <Action ID="IsUserAvailable"/>
       </Decorator>
-      <Action ID="WaitForUserTrajectoryApproval" solution="{solution}" />
+      <Action ID="WaitForUserTrajectoryApproval" solution="{solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Wait for Trajectory Approval if User Available">
       <MetadataFields>
-        <Metadata subcategory="User Input" />
+        <Metadata subcategory="User Input"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/multi_arm_sim/package.xml b/src/moveit_pro_ur_configs/multi_arm_sim/package.xml
index b9abe479..bb6c7693 100644
--- a/src/moveit_pro_ur_configs/multi_arm_sim/package.xml
+++ b/src/moveit_pro_ur_configs/multi_arm_sim/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>multi_arm_sim</name>
   <version>6.5.0</version>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/agent_bridge.launch.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/agent_bridge.launch.xml
index b2aa7543..8eeec7d9 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/agent_bridge.launch.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/agent_bridge.launch.xml
@@ -1,6 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
 </launch>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/moveit_pro.launch.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/moveit_pro.launch.xml
index 102a8a24..71fbf0bb 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/moveit_pro.launch.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/moveit_pro.launch.xml
@@ -1,11 +1,6 @@
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/robot_drivers.launch.py"
-  />
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
-  <include
-    file="$(find-pkg-share moveit_studio_rest_api)/launch/rest_api.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/robot_drivers.launch.py"/>
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
+  <include file="$(find-pkg-share moveit_studio_rest_api)/launch/rest_api.launch.xml"/>
 </launch>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/rs_cameras.launch.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/rs_cameras.launch.xml
index 285ff314..e017bd13 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/rs_cameras.launch.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/rs_cameras.launch.xml
@@ -1,74 +1,46 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <arg name="scene_camera_serial_no" default="'0'" />
-  <arg name="wrist_mounted_camera_serial_no" default="'0'" />
+  <arg name="scene_camera_serial_no" default="'0'"/>
+  <arg name="wrist_mounted_camera_serial_no" default="'0'"/>
 
   <!-- Scene camera  -->
-  <include
-    file="$(find-pkg-share picknik_ur_base_config)/launch/pro_rs_launch.py"
-  >
-    <arg name="camera_name" value="scene_camera" />
-    <arg name="serial_no" value="$(var scene_camera_serial_no)" />
-    <arg name="device_type" value="D415" />
-    <arg name="depth_module.profile" value="640x480x6" />
-    <arg name="rgb_camera.profile" value="640x480x6" />
-    <arg name="pointcloud.enable" value="True" />
-    <arg name="pointcloud.ordered_pc" value="True" />
-    <arg name="decimation_filter.enable" value="False" />
+  <include file="$(find-pkg-share picknik_ur_base_config)/launch/pro_rs_launch.py">
+    <arg name="camera_name" value="scene_camera"/>
+    <arg name="serial_no" value="$(var scene_camera_serial_no)"/>
+    <arg name="device_type" value="D415"/>
+    <arg name="depth_module.profile" value="640x480x6"/>
+    <arg name="rgb_camera.profile" value="640x480x6"/>
+    <arg name="pointcloud.enable" value="True"/>
+    <arg name="pointcloud.ordered_pc" value="True"/>
+    <arg name="decimation_filter.enable" value="False"/>
 
     <!--# information about the topics the camera publishes the raw image and info-->
-    <arg name="rgb_info" value="/scene_camera/color/camera_info" />
-    <arg name="rgb_image" value="/scene_camera/color/image_raw" />
-    <arg
-      name="registered_rgb_depth_pair.depth_info"
-      value="/scene_camera/color/camera_info"
-    />
-    <arg
-      name="registered_rgb_depth_pair.depth_image"
-      value="/scene_camera/depth/image_rect_raw"
-    />
-    <arg
-      name="registered_rgb_depth_pair.registered_info"
-      value="/scene_camera/color/camera_info"
-    />
-    <arg
-      name="registered_rgb_depth_pair.registered_image"
-      value="/scene_camera/depth/image_rect_raw"
-    />
+    <arg name="rgb_info" value="/scene_camera/color/camera_info"/>
+    <arg name="rgb_image" value="/scene_camera/color/image_raw"/>
+    <arg name="registered_rgb_depth_pair.depth_info" value="/scene_camera/color/camera_info"/>
+    <arg name="registered_rgb_depth_pair.depth_image" value="/scene_camera/depth/image_rect_raw"/>
+    <arg name="registered_rgb_depth_pair.registered_info" value="/scene_camera/color/camera_info"/>
+    <arg name="registered_rgb_depth_pair.registered_image" value="/scene_camera/depth/image_rect_raw"/>
   </include>
 
   <!-- Wrist camera  -->
-  <include
-    file="$(find-pkg-share picknik_ur_base_config)/launch/pro_rs_launch.py"
-  >
-    <arg name="camera_name" value="wrist_mounted_camera" />
-    <arg name="serial_no" value="$(var wrist_mounted_camera_serial_no)" />
-    <arg name="device_type" value="D415" />
-    <arg name="depth_module.profile" value="640x480x6" />
-    <arg name="pointcloud.enable" value="True" />
-    <arg name="pointcloud.ordered_pc" value="True" />
-    <arg name="decimation_filter.enable" value="False" />
+  <include file="$(find-pkg-share picknik_ur_base_config)/launch/pro_rs_launch.py">
+    <arg name="camera_name" value="wrist_mounted_camera"/>
+    <arg name="serial_no" value="$(var wrist_mounted_camera_serial_no)"/>
+    <arg name="device_type" value="D415"/>
+    <arg name="depth_module.profile" value="640x480x6"/>
+    <arg name="pointcloud.enable" value="True"/>
+    <arg name="pointcloud.ordered_pc" value="True"/>
+    <arg name="decimation_filter.enable" value="False"/>
 
     <!-- information about the topics the camera publishes the raw image and info -->
-    <arg name="rgb_info" value="/wrist_mounted_camera/color/camera_info" />
-    <arg name="rgb_image" value="/wrist_mounted_camera/color/image_raw" />
+    <arg name="rgb_info" value="/wrist_mounted_camera/color/camera_info"/>
+    <arg name="rgb_image" value="/wrist_mounted_camera/color/image_raw"/>
 
     <!-- By adding registered_rgb_depth_pair, This camera can be used for "Set Transform From Click" -->
-    <arg
-      name="registered_rgb_depth_pair.depth_info"
-      value="/wrist_mounted_camera/depth/camera_info"
-    />
-    <arg
-      name="registered_rgb_depth_pair.depth_image"
-      value="/wrist_mounted_camera/depth/image_rect_raw"
-    />
-    <arg
-      name="registered_rgb_depth_pair.registered_info"
-      value="/wrist_mounted_camera/depth_registered/camera_info"
-    />
-    <arg
-      name="registered_rgb_depth_pair.registered_image"
-      value="/wrist_mounted_camera/depth_registered/image_rect"
-    />
+    <arg name="registered_rgb_depth_pair.depth_info" value="/wrist_mounted_camera/depth/camera_info"/>
+    <arg name="registered_rgb_depth_pair.depth_image" value="/wrist_mounted_camera/depth/image_rect_raw"/>
+    <arg name="registered_rgb_depth_pair.registered_info" value="/wrist_mounted_camera/depth_registered/camera_info"/>
+    <arg name="registered_rgb_depth_pair.registered_image" value="/wrist_mounted_camera/depth_registered/image_rect"/>
   </include>
 </launch>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/ur_tool_comms.launch.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/ur_tool_comms.launch.xml
index 069c35f2..61e346d9 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/ur_tool_comms.launch.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/launch/ur_tool_comms.launch.xml
@@ -1,17 +1,7 @@
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <arg name="robot_ip" description="IP address of the UR robot controller." />
-  <arg
-    name="tool_tcp_port"
-    default="54321"
-    description="Port for tool communications."
-  />
-  <arg
-    name="tool_device_name"
-    description="Device name to use for tool communications."
-  />
-  <executable
-    cmd="socat pty,link=$(var tool_device_name),raw,ignoreeof,waitslave tcp:$(var robot_ip):$(var tool_tcp_port)"
-    output="both"
-    shell="true"
-  />
+  <arg name="robot_ip" description="IP address of the UR robot controller."/>
+  <arg name="tool_tcp_port" default="54321" description="Port for tool communications."/>
+  <arg name="tool_device_name" description="Device name to use for tool communications."/>
+  <executable cmd="socat pty,link=$(var tool_device_name),raw,ignoreeof,waitslave tcp:$(var robot_ip):$(var tool_tcp_port)" output="both" shell="true"/>
 </launch>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml
index 80d01106..7f6cc9b2 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml
@@ -1,25 +1,15 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Close Gripper">
-  <BehaviorTree
-    ID="Close Gripper"
-    _description="Close the gripper"
-    _favorite="true"
-    _subtreeOnly="false"
-  >
+  <BehaviorTree ID="Close Gripper" _description="Close the gripper" _favorite="true" _subtreeOnly="false">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.7929"
-        timeout="15.000000"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929" timeout="15.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Close Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Grasping"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/create_point_cloud_vector_from_masks.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/create_point_cloud_vector_from_masks.xml
index 4a42eeb2..1a7cb1c5 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/create_point_cloud_vector_from_masks.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/create_point_cloud_vector_from_masks.xml
@@ -1,49 +1,25 @@
-<?xml version="1.0" encoding="utf-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Create Point Cloud Vector From Masks"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Create Point Cloud Vector From Masks">
   <!--//////////-->
-  <BehaviorTree
-    ID="Create Point Cloud Vector From Masks"
-    _subtreeOnly="true"
-    _description="Create point cloud vector From masks"
-    _favorite="true"
-    masks3d="/wrist_camera/camera_info"
-    point_cloud="/wrist_camera/camera_info"
-  >
+  <BehaviorTree ID="Create Point Cloud Vector From Masks" _subtreeOnly="true" _description="Create point cloud vector From masks" _favorite="true" masks3d="/wrist_camera/camera_info" point_cloud="/wrist_camera/camera_info">
     <Control ID="Sequence">
-      <Action ID="ResetPoseStampedVector" vector="{pose_stamped_vector}" />
-      <Decorator
-        ID="ForEach"
-        vector_in="{masks3d}"
-        out="{mask3d}"
-        index="{index}"
-      >
+      <Action ID="ResetPoseStampedVector" vector="{pose_stamped_vector}"/>
+      <Decorator ID="ForEach" vector_in="{masks3d}" out="{mask3d}" index="{index}">
         <Control ID="Sequence">
-          <Action
-            ID="GetPointCloudFromMask3D"
-            point_cloud="{point_cloud}"
-            mask3d="{mask3d}"
-            point_cloud_fragment="{point_cloud_fragment}"
-          />
-          <Action
-            ID="AddPointCloudToVector"
-            point_cloud="{point_cloud_fragment}"
-            point_cloud_vector="{point_cloud_vector}"
-          />
+          <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
+          <Action ID="AddPointCloudToVector" point_cloud="{point_cloud_fragment}" point_cloud_vector="{point_cloud_vector}"/>
         </Control>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Create Point Cloud Vector From Masks">
-      <input_port name="masks3d" default="/wrist_camera/camera_info" />
-      <input_port name="point_cloud" default="/wrist_camera/camera_info" />
-      <output_port name="point_cloud_vector" default="{point_cloud_vector}" />
+      <input_port name="masks3d" default="/wrist_camera/camera_info"/>
+      <input_port name="point_cloud" default="/wrist_camera/camera_info"/>
+      <output_port name="point_cloud_vector" default="{point_cloud_vector}"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Perception - ML" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Perception - ML"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/open_gripper.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/open_gripper.xml
index 14921bf6..aada5976 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/open_gripper.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/open_gripper.xml
@@ -1,23 +1,15 @@
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Open Gripper">
   <!--//////////-->
-  <BehaviorTree
-    ID="Open Gripper"
-    _description="Open the gripper"
-    _favorite="false"
-  >
+  <BehaviorTree ID="Open Gripper" _description="Open the gripper" _favorite="false">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.05"
-        timeout="15.000000"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.05" timeout="15.000000"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Open Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_image_from_no_negative_text_prompt_subtree.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_image_from_no_negative_text_prompt_subtree.xml
index 3ef7ac4f..378e01da 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_image_from_no_negative_text_prompt_subtree.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_image_from_no_negative_text_prompt_subtree.xml
@@ -1,62 +1,31 @@
-<?xml version="1.0" encoding="utf-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Segment Image from No Negative Text Prompt Subtree"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Segment Image from No Negative Text Prompt Subtree">
   <!--//////////-->
-  <BehaviorTree
-    ID="Segment Image from No Negative Text Prompt Subtree"
-    _subtreeOnly="true"
-    _description="Run text based segmentation and visualize the masks."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Segment Image from No Negative Text Prompt Subtree" _subtreeOnly="true" _description="Run text based segmentation and visualize the masks." _favorite="true">
     <Control ID="Sequence">
-      <Action
-        ID="GetImage"
-        timeout_sec="5.000000"
-        topic_name="{image_topic_name}"
-        message_out="{image}"
-      />
-      <Action
-        ID="GetMasks2DFromTextQuery"
-        image="{image}"
-        masks2d="{masks2d}"
-        clipseg_model_path="{clipseg_model_path}"
-        clip_model_path="{clip_model_path}"
-        model_package="{model_package}"
-        prompts="{prompts}"
-        erosion_size="{erosion_size}"
-        threshold="{threshold}"
-      />
-      <Action
-        ID="PublishMask2D"
-        image="{image}"
-        masks="{masks2d}"
-        masks_visualization_topic="{masks_visualization_topic}"
-      />
+      <Action ID="GetImage" timeout_sec="5.000000" topic_name="{image_topic_name}" message_out="{image}"/>
+      <Action ID="GetMasks2DFromTextQuery" image="{image}" masks2d="{masks2d}" clipseg_model_path="{clipseg_model_path}" clip_model_path="{clip_model_path}" model_package="{model_package}" prompts="{prompts}" erosion_size="{erosion_size}" threshold="{threshold}"/>
+      <Action ID="PublishMask2D" image="{image}" masks="{masks2d}" masks_visualization_topic="{masks_visualization_topic}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Segment Image from No Negative Text Prompt Subtree">
-      <input_port name="clip_model_path" default="{clip_model_path}" />
-      <input_port name="clipseg_model_path" default="{clipseg_model_path}" />
-      <input_port name="erosion_size" default="{erosion_size}" />
-      <input_port
-        name="masks_visualization_topic"
-        default="{masks_visualization_topic}"
-      />
-      <input_port name="model_package" default="{model_package}" />
-      <input_port name="prompts" default="{prompts}" />
-      <input_port name="threshold" default="{threshold}" />
-      <input_port name="image_topic_name" default="{image_topic_name}" />
-      <output_port name="masks2d" default="{masks2d}" />
+      <input_port name="clip_model_path" default="{clip_model_path}"/>
+      <input_port name="clipseg_model_path" default="{clipseg_model_path}"/>
+      <input_port name="erosion_size" default="{erosion_size}"/>
+      <input_port name="masks_visualization_topic" default="{masks_visualization_topic}"/>
+      <input_port name="model_package" default="{model_package}"/>
+      <input_port name="prompts" default="{prompts}"/>
+      <input_port name="threshold" default="{threshold}"/>
+      <input_port name="image_topic_name" default="{image_topic_name}"/>
+      <output_port name="masks2d" default="{masks2d}"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Perception - ML" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Perception - ML"/>
       </MetadataFields>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Perception - ML" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Perception - ML"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_image_from_point_subtree.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_image_from_point_subtree.xml
index ed66f426..dd074f68 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_image_from_point_subtree.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_image_from_point_subtree.xml
@@ -1,66 +1,27 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Segment Image from Point Subtree">
   <!--//////////-->
-  <BehaviorTree
-    ID="Segment Image from Point Subtree"
-    _subtreeOnly="true"
-    _description="Captures an image and segments it using the user provided prompt points."
-    _favorite="true"
-    decoder_model_path="models/decoder.onnx"
-    encoder_model_path="models/sam2_hiera_large_encoder.onnx"
-    image_topic_name="/wrist_camera/color"
-    masks_visualization_topic="/masks_visualization"
-    model_package="lab_sim"
-    masks2d="{masks2d}"
-    pixel_coords="{pixel_coords}"
-  >
+  <BehaviorTree ID="Segment Image from Point Subtree" _subtreeOnly="true" _description="Captures an image and segments it using the user provided prompt points." _favorite="true" decoder_model_path="models/decoder.onnx" encoder_model_path="models/sam2_hiera_large_encoder.onnx" image_topic_name="/wrist_camera/color" masks_visualization_topic="/masks_visualization" model_package="lab_sim" masks2d="{masks2d}" pixel_coords="{pixel_coords}">
     <Control ID="Sequence">
-      <Action
-        ID="GetImage"
-        topic_name="{image_topic_name}"
-        timeout_sec="5.000000"
-        message_out="{image}"
-      />
-      <Action
-        ID="GetPointsFromUser"
-        point_prompts="Select the object to be segmented (1/3);Select the objects to be segmented (2/3);Select the objects to be segmented (3/3)"
-        point_names="Point1;Point2;Point3;"
-        view_name="{image_topic_name}"
-        pixel_coords="{pixel_coords}"
-      />
-      <Action
-        ID="GetMasks2DFromPointQuery"
-        image="{image}"
-        pixel_coords="{pixel_coords}"
-        masks2d="{masks2d}"
-        decoder_model_path="{decoder_model_path}"
-        encoder_model_path="{encoder_model_path}"
-        model_package="{model_package}"
-      />
-      <Action
-        ID="PublishMask2D"
-        image="{image}"
-        masks="{masks2d}"
-        masks_visualization_topic="{masks_visualization_topic}"
-      />
-      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
+      <Action ID="GetImage" topic_name="{image_topic_name}" timeout_sec="5.000000" message_out="{image}"/>
+      <Action ID="GetPointsFromUser" point_prompts="Select the object to be segmented (1/3);Select the objects to be segmented (2/3);Select the objects to be segmented (3/3)" point_names="Point1;Point2;Point3;" view_name="{image_topic_name}" pixel_coords="{pixel_coords}"/>
+      <Action ID="GetMasks2DFromPointQuery" image="{image}" pixel_coords="{pixel_coords}" masks2d="{masks2d}" decoder_model_path="{decoder_model_path}" encoder_model_path="{encoder_model_path}" model_package="{model_package}"/>
+      <Action ID="PublishMask2D" image="{image}" masks="{masks2d}" masks_visualization_topic="{masks_visualization_topic}"/>
+      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Segment Image from Point Subtree">
-      <input_port name="decoder_model_path" default="{decoder_model_path}" />
-      <input_port name="encoder_model_path" default="{encoder_model_path}" />
-      <input_port name="image_topic_name" default="{image_topic_name}" />
-      <input_port
-        name="masks_visualization_topic"
-        default="{masks_visualization_topic}"
-      />
-      <output_port name="masks2d" default="{masks2d}" />
-      <input_port name="model_package" default="{model_package}" />
-      <output_port name="pixel_coords" default="{pixel_coords}" />
+      <input_port name="decoder_model_path" default="{decoder_model_path}"/>
+      <input_port name="encoder_model_path" default="{encoder_model_path}"/>
+      <input_port name="image_topic_name" default="{image_topic_name}"/>
+      <input_port name="masks_visualization_topic" default="{masks_visualization_topic}"/>
+      <output_port name="masks2d" default="{masks2d}"/>
+      <input_port name="model_package" default="{model_package}"/>
+      <output_port name="pixel_coords" default="{pixel_coords}"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Perception - ML" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Perception - ML"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_image_from_text_prompt_subtree.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_image_from_text_prompt_subtree.xml
index 4865827b..886f3a87 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_image_from_text_prompt_subtree.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_image_from_text_prompt_subtree.xml
@@ -1,60 +1,28 @@
-<?xml version="1.0" encoding="utf-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Segment Image from Text Prompt Subtree"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Segment Image from Text Prompt Subtree">
   <!--//////////-->
-  <BehaviorTree
-    ID="Segment Image from Text Prompt Subtree"
-    _subtreeOnly="true"
-    _description="Run text based segmentation and visualize the masks."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Segment Image from Text Prompt Subtree" _subtreeOnly="true" _description="Run text based segmentation and visualize the masks." _favorite="true">
     <Control ID="Sequence">
-      <Action
-        ID="GetImage"
-        timeout_sec="5.000000"
-        topic_name="{image_topic_name}"
-        message_out="{image}"
-      />
-      <Action
-        ID="GetMasks2DFromTextQuery"
-        image="{image}"
-        masks2d="{masks2d}"
-        clipseg_model_path="{clipseg_model_path}"
-        clip_model_path="{clip_model_path}"
-        model_package="{model_package}"
-        negative_prompts="{negative_prompts}"
-        prompts="{prompts}"
-        erosion_size="{erosion_size}"
-        threshold="{threshold}"
-      />
-      <Action
-        ID="PublishMask2D"
-        image="{image}"
-        masks="{masks2d}"
-        masks_visualization_topic="{masks_visualization_topic}"
-      />
+      <Action ID="GetImage" timeout_sec="5.000000" topic_name="{image_topic_name}" message_out="{image}"/>
+      <Action ID="GetMasks2DFromTextQuery" image="{image}" masks2d="{masks2d}" clipseg_model_path="{clipseg_model_path}" clip_model_path="{clip_model_path}" model_package="{model_package}" negative_prompts="{negative_prompts}" prompts="{prompts}" erosion_size="{erosion_size}" threshold="{threshold}"/>
+      <Action ID="PublishMask2D" image="{image}" masks="{masks2d}" masks_visualization_topic="{masks_visualization_topic}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Segment Image from Text Prompt Subtree">
-      <input_port name="clip_model_path" default="{clip_model_path}" />
-      <input_port name="clipseg_model_path" default="{clipseg_model_path}" />
-      <input_port name="erosion_size" default="{erosion_size}" />
-      <input_port
-        name="masks_visualization_topic"
-        default="{masks_visualization_topic}"
-      />
-      <input_port name="model_package" default="{model_package}" />
-      <input_port name="negative_prompts" default="{negative_prompts}" />
-      <input_port name="prompts" default="{prompts}" />
-      <input_port name="threshold" default="{threshold}" />
-      <input_port name="image_topic_name" default="{image_topic_name}" />
-      <output_port name="masks2d" default="{masks2d}" />
+      <input_port name="clip_model_path" default="{clip_model_path}"/>
+      <input_port name="clipseg_model_path" default="{clipseg_model_path}"/>
+      <input_port name="erosion_size" default="{erosion_size}"/>
+      <input_port name="masks_visualization_topic" default="{masks_visualization_topic}"/>
+      <input_port name="model_package" default="{model_package}"/>
+      <input_port name="negative_prompts" default="{negative_prompts}"/>
+      <input_port name="prompts" default="{prompts}"/>
+      <input_port name="threshold" default="{threshold}"/>
+      <input_port name="image_topic_name" default="{image_topic_name}"/>
+      <output_port name="masks2d" default="{masks2d}"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Perception - ML" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Perception - ML"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_point_cloud_from_clicked_point_subtree.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_point_cloud_from_clicked_point_subtree.xml
index f6ae3483..b53b8391 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_point_cloud_from_clicked_point_subtree.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_point_cloud_from_clicked_point_subtree.xml
@@ -1,78 +1,28 @@
-<?xml version="1.0" encoding="utf-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Segment Point Cloud from Clicked Point Subtree"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Segment Point Cloud from Clicked Point Subtree">
   <!--//////////-->
-  <BehaviorTree
-    ID="Segment Point Cloud from Clicked Point Subtree"
-    _subtreeOnly="true"
-    _description="Captures a point cloud and requests the user to click an object in the image to be segmented. The point cloud is then filtered to only include the selected object."
-    _favorite="true"
-    camera_topic_name="{camera_topic_name}"
-    decoder_model_path="{decoder_model_path}"
-    encoder_model_path="{encoder_model_path}"
-    image_topic_name="{image_topic_name}"
-    masks_visualization_topic="{masks_visualization_topic}"
-    model_package="{model_package}"
-    points_topic_name="{points_topic_name}"
-  >
+  <BehaviorTree ID="Segment Point Cloud from Clicked Point Subtree" _subtreeOnly="true" _description="Captures a point cloud and requests the user to click an object in the image to be segmented. The point cloud is then filtered to only include the selected object." _favorite="true" camera_topic_name="{camera_topic_name}" decoder_model_path="{decoder_model_path}" encoder_model_path="{encoder_model_path}" image_topic_name="{image_topic_name}" masks_visualization_topic="{masks_visualization_topic}" model_package="{model_package}" points_topic_name="{points_topic_name}">
     <Control ID="Sequence">
-      <Action ID="ClearSnapshot" />
-      <SubTree
-        ID="Segment Image from Point Subtree"
-        _collapsed="true"
-        decoder_model_path="{decoder_model_path}"
-        encoder_model_path="{encoder_model_path}"
-        image_topic="{image_topic_name}"
-        masks_visualization_topic="{masks_visualization_topic}"
-        model_package="{model_package}"
-        pixel_coords="{pixel_coords}"
-        masks2d="{masks2d}"
-        image_topic_name="{image_topic_name}"
-      />
-      <Action
-        ID="GetPointCloud"
-        topic_name="{points_topic_name}"
-        timeout_sec="5.000000"
-        message_out="{point_cloud}"
-      />
-      <Action
-        ID="GetCameraInfo"
-        topic_name="{camera_topic_name}"
-        message_out="{camera_info}"
-        timeout_sec="5.000000"
-      />
-      <Action
-        ID="GetMasks3DFromMasks2D"
-        camera_info="{camera_info}"
-        masks2d="{masks2d}"
-        point_cloud="{point_cloud}"
-        masks3d="{masks3d}"
-      />
-      <SubTree
-        ID="Visualize Segmented Point Cloud"
-        _collapsed="true"
-        masks3d="{masks3d}"
-        point_cloud="{point_cloud}"
-      />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Segment Image from Point Subtree" _collapsed="true" decoder_model_path="{decoder_model_path}" encoder_model_path="{encoder_model_path}" image_topic="{image_topic_name}" masks_visualization_topic="{masks_visualization_topic}" model_package="{model_package}" pixel_coords="{pixel_coords}" masks2d="{masks2d}" image_topic_name="{image_topic_name}"/>
+      <Action ID="GetPointCloud" topic_name="{points_topic_name}" timeout_sec="5.000000" message_out="{point_cloud}"/>
+      <Action ID="GetCameraInfo" topic_name="{camera_topic_name}" message_out="{camera_info}" timeout_sec="5.000000"/>
+      <Action ID="GetMasks3DFromMasks2D" camera_info="{camera_info}" masks2d="{masks2d}" point_cloud="{point_cloud}" masks3d="{masks3d}"/>
+      <SubTree ID="Visualize Segmented Point Cloud" _collapsed="true" masks3d="{masks3d}" point_cloud="{point_cloud}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Segment Point Cloud from Clicked Point Subtree">
-      <input_port name="camera_topic_name" default="{camera_topic_name}" />
-      <input_port name="decoder_model_path" default="{decoder_model_path}" />
-      <input_port name="encoder_model_path" default="{encoder_model_path}" />
-      <input_port name="image_topic_name" default="{image_topic_name}" />
-      <input_port
-        name="masks_visualization_topic"
-        default="{masks_visualization_topic}"
-      />
-      <input_port name="model_package" default="{model_package}" />
-      <input_port name="points_topic_name" default="{points_topic_name}" />
+      <input_port name="camera_topic_name" default="{camera_topic_name}"/>
+      <input_port name="decoder_model_path" default="{decoder_model_path}"/>
+      <input_port name="encoder_model_path" default="{encoder_model_path}"/>
+      <input_port name="image_topic_name" default="{image_topic_name}"/>
+      <input_port name="masks_visualization_topic" default="{masks_visualization_topic}"/>
+      <input_port name="model_package" default="{model_package}"/>
+      <input_port name="points_topic_name" default="{points_topic_name}"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Perception - ML" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Perception - ML"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_point_cloud_from_text_prompt_subtree.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_point_cloud_from_text_prompt_subtree.xml
index 53142ee8..db1edac3 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_point_cloud_from_text_prompt_subtree.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/segment_point_cloud_from_text_prompt_subtree.xml
@@ -1,114 +1,39 @@
-<?xml version="1.0" encoding="utf-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Segment Point Cloud from Text Prompt Subtree"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Segment Point Cloud from Text Prompt Subtree">
   <!--//////////-->
-  <BehaviorTree
-    ID="Segment Point Cloud from Text Prompt Subtree"
-    _subtreeOnly="true"
-    _description="Captures a point cloud and requests the user to click an object in the image to be segmented. The point cloud is then filtered to only include the selected object."
-    _favorite="true"
-    camera_topic_name="/wrist_camera/camera_info"
-    decoder_model_path="models/decoder.onnx"
-    encoder_model_path="models/sam2_hiera_large_encoder.onnx"
-    image_topic_name="/wrist_camera/color"
-    masks_visualization_topic="/masks_visualization"
-    model_package="lab_sim"
-    points_topic_name="/wrist_camera/points"
-    clip_model_path="{clip_model_path}"
-    clipseg_model_path="{clipseg_model_path}"
-    erosion_size="{erosion_size}"
-    prompts="{prompts}"
-    threshold="{threshold}"
-    masks3d="{masks3d}"
-    point_cloud="{point_cloud}"
-  >
+  <BehaviorTree ID="Segment Point Cloud from Text Prompt Subtree" _subtreeOnly="true" _description="Captures a point cloud and requests the user to click an object in the image to be segmented. The point cloud is then filtered to only include the selected object." _favorite="true" camera_topic_name="/wrist_camera/camera_info" decoder_model_path="models/decoder.onnx" encoder_model_path="models/sam2_hiera_large_encoder.onnx" image_topic_name="/wrist_camera/color" masks_visualization_topic="/masks_visualization" model_package="lab_sim" points_topic_name="/wrist_camera/points" clip_model_path="{clip_model_path}" clipseg_model_path="{clipseg_model_path}" erosion_size="{erosion_size}" prompts="{prompts}" threshold="{threshold}" masks3d="{masks3d}" point_cloud="{point_cloud}">
     <Control ID="Sequence">
-      <Action ID="ClearSnapshot" />
-      <SubTree
-        ID="Segment Image from No Negative Text Prompt Subtree"
-        _collapsed="true"
-        clip_model_path="{clip_model_path}"
-        clipseg_model_path="{clipseg_model_path}"
-        erosion_size="{erosion_size}"
-        masks_visualization_topic="{masks_visualization_topic}"
-        model_package="{model_package}"
-        prompts="{prompts}"
-        threshold="{threshold}"
-        topic_name="{topic_name}"
-        image_topic_name="{image_topic_name}"
-        masks2d="{masks2d}"
-      />
-      <Action
-        ID="GetPointCloud"
-        topic_name="{points_topic_name}"
-        timeout_sec="5.000000"
-        message_out="{point_cloud}"
-      />
-      <Action
-        ID="GetCameraInfo"
-        topic_name="{camera_topic_name}"
-        message_out="{camera_info}"
-        timeout_sec="5.000000"
-      />
-      <Action
-        ID="GetMasks3DFromMasks2D"
-        camera_info="{camera_info}"
-        masks2d="{masks2d}"
-        point_cloud="{point_cloud}"
-        masks3d="{masks3d}"
-      />
-      <SubTree
-        ID="Create Point Cloud Vector From Masks"
-        _collapsed="false"
-        masks3d="{masks3d}"
-        point_cloud="{point_cloud}"
-        point_cloud_vector="{point_cloud_vector}"
-      />
-      <Decorator
-        ID="ForEach"
-        index="{index}"
-        out="{point_cloud_fragment}"
-        vector_in="{point_cloud_vector}"
-      >
-        <Action
-          ID="SendPointCloudToUI"
-          pcd_topic="/pcd_pointcloud_captures"
-          point_cloud="{point_cloud_fragment}"
-        />
+      <Action ID="ClearSnapshot"/>
+      <SubTree ID="Segment Image from No Negative Text Prompt Subtree" _collapsed="true" clip_model_path="{clip_model_path}" clipseg_model_path="{clipseg_model_path}" erosion_size="{erosion_size}" masks_visualization_topic="{masks_visualization_topic}" model_package="{model_package}" prompts="{prompts}" threshold="{threshold}" topic_name="{topic_name}" image_topic_name="{image_topic_name}" masks2d="{masks2d}"/>
+      <Action ID="GetPointCloud" topic_name="{points_topic_name}" timeout_sec="5.000000" message_out="{point_cloud}"/>
+      <Action ID="GetCameraInfo" topic_name="{camera_topic_name}" message_out="{camera_info}" timeout_sec="5.000000"/>
+      <Action ID="GetMasks3DFromMasks2D" camera_info="{camera_info}" masks2d="{masks2d}" point_cloud="{point_cloud}" masks3d="{masks3d}"/>
+      <SubTree ID="Create Point Cloud Vector From Masks" _collapsed="false" masks3d="{masks3d}" point_cloud="{point_cloud}" point_cloud_vector="{point_cloud_vector}"/>
+      <Decorator ID="ForEach" index="{index}" out="{point_cloud_fragment}" vector_in="{point_cloud_vector}">
+        <Action ID="SendPointCloudToUI" pcd_topic="/pcd_pointcloud_captures" point_cloud="{point_cloud_fragment}"/>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Segment Point Cloud from Text Prompt Subtree">
-      <input_port
-        name="camera_topic_name"
-        default="/wrist_camera/camera_info"
-      />
-      <input_port name="clip_model_path" default="{clip_model_path}" />
-      <input_port name="clipseg_model_path" default="{clipseg_model_path}" />
-      <input_port name="decoder_model_path" default="models/decoder.onnx" />
-      <input_port
-        name="encoder_model_path"
-        default="models/sam2_hiera_large_encoder.onnx"
-      />
-      <input_port name="erosion_size" default="{erosion_size}" />
-      <input_port name="image_topic_name" default="/wrist_camera/color" />
-      <input_port
-        name="masks_visualization_topic"
-        default="/masks_visualization"
-      />
-      <output_port name="masks3d" default="{masks3d}" />
-      <input_port name="model_package" default="lab_sim" />
-      <output_port name="point_cloud" default="{point_cloud}" />
-      <output_port name="point_cloud_vector" default="{point_cloud_vector}" />
-      <input_port name="points_topic_name" default="/wrist_camera/points" />
-      <input_port name="prompts" default="{prompts}" />
-      <input_port name="threshold" default="{threshold}" />
+      <input_port name="camera_topic_name" default="/wrist_camera/camera_info"/>
+      <input_port name="clip_model_path" default="{clip_model_path}"/>
+      <input_port name="clipseg_model_path" default="{clipseg_model_path}"/>
+      <input_port name="decoder_model_path" default="models/decoder.onnx"/>
+      <input_port name="encoder_model_path" default="models/sam2_hiera_large_encoder.onnx"/>
+      <input_port name="erosion_size" default="{erosion_size}"/>
+      <input_port name="image_topic_name" default="/wrist_camera/color"/>
+      <input_port name="masks_visualization_topic" default="/masks_visualization"/>
+      <output_port name="masks3d" default="{masks3d}"/>
+      <input_port name="model_package" default="lab_sim"/>
+      <output_port name="point_cloud" default="{point_cloud}"/>
+      <output_port name="point_cloud_vector" default="{point_cloud_vector}"/>
+      <input_port name="points_topic_name" default="/wrist_camera/points"/>
+      <input_port name="prompts" default="{prompts}"/>
+      <input_port name="threshold" default="{threshold}"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Perception - ML" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Perception - ML"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/visualize_segmented_point_cloud.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/visualize_segmented_point_cloud.xml
index bd4b1c1e..c546bf45 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/visualize_segmented_point_cloud.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/visualize_segmented_point_cloud.xml
@@ -1,49 +1,24 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Visualize Segmented Point Cloud">
   <!--//////////-->
-  <BehaviorTree
-    ID="Visualize Segmented Point Cloud"
-    _subtreeOnly="true"
-    _description="Publish segmented point cloud to the UI."
-    _favorite="true"
-    masks3d="/wrist_camera/camera_info"
-    point_cloud="/wrist_camera/camera_info"
-  >
+  <BehaviorTree ID="Visualize Segmented Point Cloud" _subtreeOnly="true" _description="Publish segmented point cloud to the UI." _favorite="true" masks3d="/wrist_camera/camera_info" point_cloud="/wrist_camera/camera_info">
     <Control ID="Sequence">
-      <Decorator
-        ID="ForEach"
-        vector_in="{masks3d}"
-        out="{mask3d}"
-        index="{index}"
-      >
+      <Decorator ID="ForEach" vector_in="{masks3d}" out="{mask3d}" index="{index}">
         <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}"
-            pcd_topic="/pcd_pointcloud_captures"
-          />
-          <Action
-            ID="PublishPointCloud"
-            point_cloud="{point_cloud_fragment}"
-            point_cloud_topic="/my_point_cloud"
-          />
+          <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
+          <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" pcd_topic="/pcd_pointcloud_captures"/>
+          <Action ID="PublishPointCloud" point_cloud="{point_cloud_fragment}" point_cloud_topic="/my_point_cloud"/>
         </Control>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Visualize Segmented Point Cloud">
-      <input_port name="masks3d" default="{masks3d}" />
-      <input_port name="point_cloud" default="{point_cloud}" />
+      <input_port name="masks3d" default="{masks3d}"/>
+      <input_port name="point_cloud" default="{point_cloud}"/>
       <MetadataFields>
-        <Metadata runnable="false" />
-        <Metadata subcategory="Perception - ML" />
+        <Metadata runnable="false"/>
+        <Metadata subcategory="Perception - ML"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/package.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/package.xml
index 6fe4d9e0..506cf411 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/package.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>picknik_ur_base_config</name>
   <version>6.5.0</version>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/behaviors/trigger_pstop_reset_service_plugin_description.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/behaviors/trigger_pstop_reset_service_plugin_description.xml
index 5574332f..4280d756 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/behaviors/trigger_pstop_reset_service_plugin_description.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/behaviors/trigger_pstop_reset_service_plugin_description.xml
@@ -1,7 +1,4 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <library path="trigger_pstop_reset_service">
-  <class
-    type="trigger_pstop_reset_service::TriggerPStopResetServiceBehaviorsLoader"
-    base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase"
-  />
+  <class type="trigger_pstop_reset_service::TriggerPStopResetServiceBehaviorsLoader" base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase"/>
 </library>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/launch/agent_bridge.launch.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/launch/agent_bridge.launch.xml
index b2aa7543..8eeec7d9 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/launch/agent_bridge.launch.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/launch/agent_bridge.launch.xml
@@ -1,6 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <launch>
-  <include
-    file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
-  />
+  <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
 </launch>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/close_cabinet_door.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/close_cabinet_door.xml
index 13cb270a..4a2eaac6 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/close_cabinet_door.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/close_cabinet_door.xml
@@ -1,128 +1,43 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Close Cabinet Door">
-  <BehaviorTree
-    ID="Close Cabinet Door"
-    _description="Close a cabinet door by pushing along an axis with the end effector"
-  >
+  <BehaviorTree ID="Close Cabinet Door" _description="Close a cabinet door by pushing along an axis with the end effector">
     <Control ID="Sequence">
-      <SubTree ID="Re-Zero Force-Torque Sensors" />
-      <SubTree ID="Close Gripper" />
-      <Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}" />
+      <SubTree ID="Re-Zero Force-Torque Sensors"/>
+      <SubTree ID="Close Gripper"/>
+      <Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}"/>
       <Control ID="Sequence">
-        <Action
-          ID="InitializeMTCTask"
-          task_id="close_cabinet_door"
-          controller_names="/joint_trajectory_controller"
-          task="{close_cabinet_door_task}"
-        />
-        <Action ID="SetupMTCCurrentState" task="{close_cabinet_door_task}" />
-        <Action
-          ID="GetPointsFromUser"
-          view_name="/wrist_mounted_camera/color/image_raw"
-          point_names="target_pose"
-          point_prompts="Click on the door you want to close"
-          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="InitializeMTCTask" task_id="close_cabinet_door" controller_names="/joint_trajectory_controller" task="{close_cabinet_door_task}"/>
+        <Action ID="SetupMTCCurrentState" task="{close_cabinet_door_task}"/>
+        <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="target_pose" point_prompts="Click on the door you want to close" 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}"/>
         <Decorator ID="ForEach" vector_in="{output_poses}" out="{pose}">
           <Control ID="Sequence">
-            <Action
-              ID="TransformPose"
-              output_pose="{target_pose}"
-              quaternion_xyzw="0.7071;0.7071;0;0"
-              translation_xyz="0;0;0.1"
-              input_pose="{pose}"
-            />
-            <Action
-              ID="SetupMTCPlanToPose"
-              ik_frame="grasp_link"
-              planning_group_name="manipulator"
-              target_pose="{target_pose}"
-              task="{close_cabinet_door_task}"
-              planner_interface="moveit_default"
-            />
+            <Action ID="TransformPose" output_pose="{target_pose}" quaternion_xyzw="0.7071;0.7071;0;0" translation_xyz="0;0;0.1" input_pose="{pose}"/>
+            <Action ID="SetupMTCPlanToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" planner_interface="moveit_default"/>
           </Control>
         </Decorator>
-        <Action
-          ID="SetupMTCUpdateGroupCollisionRule"
-          name="AllowGripperCollisionWithOctomap"
-          group_name="gripper"
-          object_name="&lt;octomap&gt;"
-          allow_collision="true"
-          task="{close_cabinet_door_task}"
-        />
-        <Action
-          ID="SetupMTCMoveAlongFrameAxis"
-          task="{close_cabinet_door_task}"
-          hand_frame="grasp_link"
-          axis_frame="grasp_link"
-          axis_x="0.0"
-          axis_y="0.0"
-          axis_z="1.0"
-          max_distance="1.5"
-          min_distance="0.05"
-          planning_group_name="manipulator"
-          velocity_scale="0.05"
-          acceleration_scale="0.05"
-        />
-        <Action
-          ID="SetupMTCUpdateGroupCollisionRule"
-          name="ForbidGripperCollisionWithOctomap"
-          group_name="gripper"
-          object_name="&lt;octomap&gt;"
-          allow_collision="false"
-          task="{close_cabinet_door_task}"
-        />
-        <Action
-          ID="PlanMTCTask"
-          solution="{full_close_cabinet_door_solution}"
-          task="{close_cabinet_door_task}"
-        />
-        <Action
-          ID="SplitMTCSolution"
-          solution_in="{full_close_cabinet_door_solution}"
-          index="3"
-          solution_out_1="{move_to_approach_solution}"
-          solution_out_2="{push_solution}"
-        />
-        <Action
-          ID="WaitForUserTrajectoryApproval"
-          solution="{full_close_cabinet_door_solution}"
-        />
-        <Action ID="ExecuteMTCTask" solution="{move_to_approach_solution}" />
-        <Action
-          ID="SaveCurrentState"
-          saved_robot_state="{pre_approach_robot_state}"
-        />
+        <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" group_name="gripper" object_name="&lt;octomap&gt;" allow_collision="true" task="{close_cabinet_door_task}"/>
+        <Action ID="SetupMTCMoveAlongFrameAxis" task="{close_cabinet_door_task}" hand_frame="grasp_link" axis_frame="grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05"/>
+        <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" group_name="gripper" object_name="&lt;octomap&gt;" allow_collision="false" task="{close_cabinet_door_task}"/>
+        <Action ID="PlanMTCTask" solution="{full_close_cabinet_door_solution}" task="{close_cabinet_door_task}"/>
+        <Action ID="SplitMTCSolution" solution_in="{full_close_cabinet_door_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}"/>
+        <Action ID="WaitForUserTrajectoryApproval" solution="{full_close_cabinet_door_solution}"/>
+        <Action ID="ExecuteMTCTask" solution="{move_to_approach_solution}"/>
+        <Action ID="SaveCurrentState" saved_robot_state="{pre_approach_robot_state}"/>
         <Control ID="Parallel" success_count="1" failure_count="1">
-          <SubTree ID="Force Exceeds Threshold" />
-          <Action ID="ExecuteMTCTask" solution="{push_solution}" />
+          <SubTree ID="Force Exceeds Threshold"/>
+          <Action ID="ExecuteMTCTask" solution="{push_solution}"/>
         </Control>
       </Control>
-      <SubTree
-        ID="Retreat To Initial Pose"
-        pre_approach_robot_state="{pre_approach_robot_state}"
-        initial_robot_state="{initial_robot_state}"
-      />
+      <SubTree ID="Retreat To Initial Pose" pre_approach_robot_state="{pre_approach_robot_state}" initial_robot_state="{initial_robot_state}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Close Cabinet Door">
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Motion - Execute"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_door_graspable_object.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_door_graspable_object.xml
index 8dab87c9..6f8b6fe5 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_door_graspable_object.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_door_graspable_object.xml
@@ -1,89 +1,31 @@
-<?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="Segments out a door from an input depth image and attempts to fit an output 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 -->
-      <Action
-        ID="GetCameraInfo"
-        topic_name="/wrist_mounted_camera/color/camera_info"
-        message_out="{camera_info}"
-      />
-      <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="SendPointCloudToUI"
-        point_cloud="{point_cloud}"
-        pcd_topic="/pcd_pointcloud_captures"
-      />
-      <Action
-        ID="GetImage"
-        topic_name="/wrist_mounted_camera/color/image_raw"
-        message_out="{image}"
-      />
+      <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
+      <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="SendPointCloudToUI" point_cloud="{point_cloud}" 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="GetMasks3DFromMasks2D"
-        point_cloud="{point_cloud}"
-        masks2d="{masks2d}"
-        camera_info="{camera_info}"
-        masks3d="{masks3d}"
-      />
+      <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="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
       <Decorator ID="ForEach" 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}"
-            pcd_topic="/pcd_pointcloud_captures"
-          />
+          <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
+          <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" 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}"
-        />
+        <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>
       <Decorator ID="ForceSuccess">
         <Decorator ID="ForEach" vector_in="{door_objects}" out="{door_object}">
-          <Action ID="AlwaysFailure" />
+          <Action ID="AlwaysFailure"/>
         </Decorator>
       </Decorator>
     </Control>
@@ -91,7 +33,7 @@
   <TreeNodesModel>
     <SubTree ID="Detect Door Graspable Object">
       <MetadataFields>
-        <Metadata subcategory="Perception - Machine Learning" />
+        <Metadata subcategory="Perception - Machine Learning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_fixed_handle_graspable_object.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_fixed_handle_graspable_object.xml
index af1bc821..b7985a5b 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_fixed_handle_graspable_object.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_fixed_handle_graspable_object.xml
@@ -1,114 +1,41 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Detect Fixed Handle Graspable Object"
->
+<?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."
-  >
+  <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.">
     <!-- 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"
-        />
+        <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="15.0"
-        masks2d="{handle_masks2d}"
-        valid_classes="pull handle"
-      />
-      <Action
-        ID="GetMasks3DFromMasks2D"
-        point_cloud="{point_cloud}"
-        masks2d="{handle_masks2d}"
-        camera_info="{camera_info}"
-        masks3d="{masks3d}"
-      />
+      <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="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{handle_masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
       <Decorator ID="ForEach" 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}"
-            pcd_topic="/pcd_pointcloud_captures"
-          />
+          <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
+          <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" 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_objects}"
-      />
+      <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_objects}"/>
       <Decorator ID="ForceSuccess">
-        <Decorator
-          ID="ForEach"
-          vector_in="{handle_objects}"
-          out="{handle_object}"
-        >
-          <Action ID="AlwaysFailure" />
+        <Decorator ID="ForEach" vector_in="{handle_objects}" out="{handle_object}">
+          <Action ID="AlwaysFailure"/>
         </Decorator>
       </Decorator>
-      <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"
-      />
+      <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>
   <TreeNodesModel>
     <SubTree ID="Detect Fixed Handle Graspable Object">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_hinge_line_segment.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_hinge_line_segment.xml
index d39a38cc..8efc4d9f 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_hinge_line_segment.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_hinge_line_segment.xml
@@ -1,77 +1,30 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Detect Hinge Line Segment">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Detect Hinge Line Segment"
-    _description="Detects a Hinge from a depth image and fits a line segment to it, then returns the line end points."
-  >
+  <BehaviorTree ID="Detect Hinge Line Segment" _description="Detects a Hinge from a depth image and fits a line segment to it, then returns the line end points.">
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Get the point cloud from the wrist camera -->
-      <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"
-      />
+      <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"/>
       <!-- Segment out 2D masks and then convert to 3D masks -->
-      <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="{hinge_masks2d}"
-        valid_classes="space booth cabinet hinge"
-      />
-      <Action
-        ID="GetMasks3DFromMasks2D"
-        point_cloud="{point_cloud}"
-        masks2d="{hinge_masks2d}"
-        camera_info="{camera_info}"
-        masks3d="{masks3d}"
-      />
+      <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="{hinge_masks2d}" valid_classes="space booth cabinet hinge"/>
+      <Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{hinge_masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
       <Decorator ID="ForceSuccess">
         <Decorator ID="ForEach" vector_in="{masks3d}" out="{mask3d}">
-          <Action ID="AlwaysFailure" />
+          <Action ID="AlwaysFailure"/>
         </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="LogMessage"
-        message="Fit a line segment to the detected hinge successfully."
-        log_level="info"
-      />
+      <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="LogMessage" message="Fit a line segment to the detected hinge successfully." log_level="info"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Detect Hinge Line Segment">
       <MetadataFields>
-        <Metadata subcategory="Perception - Machine Learning" />
+        <Metadata subcategory="Perception - Machine Learning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_lever_handle_line_segment.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_lever_handle_line_segment.xml
index d1e65715..3eaabfb5 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_lever_handle_line_segment.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/detect_lever_handle_line_segment.xml
@@ -1,89 +1,33 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?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."
-  >
+  <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"
-        />
+        <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}"
-      />
+      <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="ForEach" 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}"
-            pcd_topic="/pcd_pointcloud_captures"
-          />
+          <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
+          <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" 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"
-      />
+      <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>
   <TreeNodesModel>
     <SubTree ID="Detect Lever Handle Line Segment">
       <MetadataFields>
-        <Metadata subcategory="Perception - Machine Learning" />
+        <Metadata subcategory="Perception - Machine Learning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/find_cuboids.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/find_cuboids.xml
index de8674f7..a752b5a5 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/find_cuboids.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/find_cuboids.xml
@@ -1,32 +1,17 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Find Cuboids">
-  <BehaviorTree
-    ID="Find Cuboids"
-    _description="Detect and move to cuboid object in current scene"
-  >
+  <BehaviorTree ID="Find Cuboids" _description="Detect and move to cuboid object in current scene">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="find_cuboids_config.yaml"
-        parameters="{parameters}"
-      />
-      <Action
-        ID="GetPointCloud"
-        topic_name="/wrist_mounted_camera/depth/color/points"
-        message_out="{point_cloud}"
-      />
-      <Action
-        ID="FindSingularCuboids"
-        point_cloud="{point_cloud}"
-        parameters="{parameters}"
-      />
+      <Action ID="LoadObjectiveParameters" config_file_name="find_cuboids_config.yaml" parameters="{parameters}"/>
+      <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
+      <Action ID="FindSingularCuboids" point_cloud="{point_cloud}" parameters="{parameters}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Find Cuboids">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Basic Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/force_exceeds_threshold.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/force_exceeds_threshold.xml
index d1d1b06a..d4c1c431 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/force_exceeds_threshold.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/force_exceeds_threshold.xml
@@ -1,25 +1,14 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Force Exceeds Threshold">
-  <BehaviorTree
-    ID="Force Exceeds Threshold"
-    _description="Uses a Force Torque Sensor topic to check if input End Effector force exceeds threshold. This subtree is used to overwrite these values for each package."
-    _favorite="false"
-  >
+  <BehaviorTree ID="Force Exceeds Threshold" _description="Uses a Force Torque Sensor topic to check if input End Effector force exceeds threshold. This subtree is used to overwrite these values for each package." _favorite="false">
     <Control ID="Sequence">
-      <Action
-        ID="ForceExceedsThreshold"
-        wrench_topic_name="/force_torque_sensor_broadcaster/wrench"
-        hand_frame_name="grasp_link"
-        wrench_frame_name="tool0"
-        minimum_consecutive_wrench_values="20"
-        force_threshold="30.0"
-      />
+      <Action ID="ForceExceedsThreshold" wrench_topic_name="/force_torque_sensor_broadcaster/wrench" hand_frame_name="grasp_link" wrench_frame_name="tool0" minimum_consecutive_wrench_values="20" force_threshold="30.0"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Force Exceeds Threshold">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/inspect_surface.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/inspect_surface.xml
index 9739a788..eccfba62 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/inspect_surface.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/inspect_surface.xml
@@ -1,72 +1,27 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Inspect Surface" _favorite="true">
-  <BehaviorTree
-    ID="Inspect Surface"
-    _description="Move the wrist camera to point towards a selected surface"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Inspect Surface" _description="Move the wrist camera to point towards a selected surface" _favorite="true">
     <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="inspect_surface"
-        controller_names="/joint_trajectory_controller"
-        task="{move_to_pose_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
-      <Action
-        ID="GetPointsFromUser"
-        view_name="/wrist_mounted_camera/color/image_raw"
-        point_names="target_pose"
-        point_prompts="Select surface to inspect"
-        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="InitializeMTCTask" task_id="inspect_surface" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
+      <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="target_pose" point_prompts="Select surface to inspect" 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}"/>
       <Decorator ID="ForEach" vector_in="{output_poses}" out="{pose}">
         <Control ID="Sequence">
-          <Action
-            ID="TransformPose"
-            output_pose="{target_pose}"
-            quaternion_xyzw="0.7071;0.7071;0;0"
-            translation_xyz="0;0;0.15"
-            input_pose="{pose}"
-          />
-          <Action
-            ID="SetupMTCPlanToPose"
-            ik_frame="grasp_link"
-            planning_group_name="manipulator"
-            target_pose="{target_pose}"
-            task="{move_to_pose_task}"
-          />
+          <Action ID="TransformPose" output_pose="{target_pose}" quaternion_xyzw="0.7071;0.7071;0;0" translation_xyz="0;0;0.15" input_pose="{pose}"/>
+          <Action ID="SetupMTCPlanToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}"/>
         </Control>
       </Decorator>
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_pose_solution}"
-        task="{move_to_pose_task}"
-      />
-      <SubTree
-        ID="Wait for Trajectory Approval if User Available"
-        solution="{move_to_pose_solution}"
-      />
-      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
+      <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+      <SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Inspect Surface">
       <MetadataFields>
-        <Metadata subcategory="Perception - Machine Learning" />
+        <Metadata subcategory="Perception - Machine Learning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/loop_detect_apriltag.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/loop_detect_apriltag.xml
index 5d17a817..c3415081 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/loop_detect_apriltag.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/loop_detect_apriltag.xml
@@ -1,37 +1,15 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Loop Detect AprilTag">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Loop Detect AprilTag"
-    _description="Loops finding April Tag markers until failure."
-    _favorite="false"
-  >
+  <BehaviorTree ID="Loop Detect AprilTag" _description="Loops finding April Tag markers until failure." _favorite="false">
     <Control ID="Sequence">
       <!-- Keep executing until failure -->
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence" name="GetDetection">
-          <Action
-            ID="LoadObjectiveParameters"
-            config_file_name="51mm_apriltag_detection_config.yaml"
-            parameters="{parameters}"
-          />
-          <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="DetectAprilTags"
-            image="{image}"
-            camera_info="{camera_info}"
-            parameters="{parameters}"
-            detections="{detections}"
-          />
+          <Action ID="LoadObjectiveParameters" config_file_name="51mm_apriltag_detection_config.yaml" parameters="{parameters}"/>
+          <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="DetectAprilTags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/>
         </Control>
       </Decorator>
     </Control>
@@ -39,7 +17,7 @@
   <TreeNodesModel>
     <SubTree ID="Loop Detect AprilTag">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/looping_pick_and_place_object.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/looping_pick_and_place_object.xml
index ed54cd0e..6711beec 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/looping_pick_and_place_object.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/looping_pick_and_place_object.xml
@@ -1,200 +1,94 @@
-<?xml version="1.0" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="Looping Pick and Place Object"
-  _favorite="true"
->
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Looping Pick and Place Object" _favorite="true">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Looping Pick and Place Object"
-    _description="Repeatedly attempt to detect and grasp a cuboid from a target bin, and drop it in the target area."
-    _favorite="true"
-  >
+  <BehaviorTree ID="Looping Pick and Place Object" _description="Repeatedly attempt to detect and grasp a cuboid from a target bin, and drop it in the target area." _favorite="true">
     <Control ID="Sequence">
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="looping_pick_and_place_object_config.yaml"
-        parameters="{parameters}"
-      />
+      <Action ID="LoadObjectiveParameters" config_file_name="looping_pick_and_place_object_config.yaml" parameters="{parameters}"/>
 
       <!-- Remove any objects which might remain in the planning scene from previous pick attempts -->
-      <Action
-        ID="ResetPlanningSceneObjects"
-        apply_planning_scene_service="apply_planning_scene"
-      />
+      <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="apply_planning_scene"/>
 
       <!-- Open the gripper and move to a consistent state. -->
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.0"
-      />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Look at Pick and Place Zone"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
+      <SubTree ID="Move to Waypoint" waypoint_name="Look at Pick and Place Zone" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
 
       <!-- This loops forever because the behavior tree under this decorator never returns FAILURE. -->
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Fallback" name="TryGraspOuter">
           <Decorator ID="KeepRunningUntilFailure">
-            <Decorator
-              ID="ForEach"
-              name="IterateThroughPlaceWaypoints"
-              vector_in="Place Right;Place Left"
-              out="{place_waypoint}"
-            >
+            <Decorator ID="ForEach" name="IterateThroughPlaceWaypoints" vector_in="Place Right;Place Left" out="{place_waypoint}">
               <Control ID="Sequence">
                 <!-- This decorator contains the logic to detect an object and then execute motion to grasp it. If grasping is aborted because the object changed while the robot was moving towards it, we will retry up to 5 times before giving up. -->
                 <Decorator ID="RetryUntilSuccessful" num_attempts="5">
                   <Control ID="Fallback" name="TryGraspInner">
                     <Control ID="Sequence">
                       <!-- Clear the collision octree. -->
-                      <Action ID="ClearSnapshot" />
+                      <Action ID="ClearSnapshot"/>
 
                       <!-- Remove any objects which might remain in the planning scene from previous pick attempts -->
-                      <Action
-                        ID="ResetPlanningSceneObjects"
-                        apply_planning_scene_service="apply_planning_scene"
-                      />
+                      <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="apply_planning_scene"/>
 
                       <!-- Get the latest point cloud from the target topic-->
-                      <Action
-                        ID="GetPointCloud"
-                        topic_name="/wrist_mounted_camera/depth/color/points"
-                        message_out="{point_cloud}"
-                      />
+                      <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
 
                       <!-- Detect all cuboids in that point cloud-->
-                      <Action
-                        ID="FindSingularCuboids"
-                        point_cloud="{point_cloud}"
-                        parameters="{parameters}"
-                        detected_shapes="{cuboids}"
-                      />
+                      <Action ID="FindSingularCuboids" point_cloud="{point_cloud}" parameters="{parameters}" detected_shapes="{cuboids}"/>
 
                       <!-- Get the first detected cuboid. NOTE: this is a small hack to get ForEach to iterate just once, which returns the first object in the vector. -->
                       <Control ID="Sequence">
                         <Decorator ID="ForceSuccess">
-                          <Decorator
-                            ID="ForEach"
-                            vector_in="{cuboids}"
-                            out="{cuboid_object}"
-                          >
+                          <Decorator ID="ForEach" vector_in="{cuboids}" out="{cuboid_object}">
                             <Control ID="Sequence">
-                              <Action ID="AlwaysFailure" />
+                              <Action ID="AlwaysFailure"/>
                             </Control>
                           </Decorator>
                         </Decorator>
                       </Control>
 
                       <!-- Add this cuboid to the planning scene. -->
-                      <Action
-                        ID="ModifyObjectInPlanningScene"
-                        object="{cuboid_object}"
-                        apply_planning_scene_service="/apply_planning_scene"
-                      />
+                      <Action ID="ModifyObjectInPlanningScene" object="{cuboid_object}" apply_planning_scene_service="/apply_planning_scene"/>
 
                       <!-- Use point cloud to update the collision octomap (happens after ModifyObjectInPlanningScene so the cuboid is excluded from the octomap)-->
-                      <Action
-                        ID="UpdatePlanningSceneService"
-                        point_cloud="{point_cloud}"
-                        point_cloud_service="/point_cloud_service"
-                      />
+                      <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
 
                       <!-- Set up an MTC task to pick the cuboid and then plan the task. -->
-                      <Action
-                        ID="InitializeMTCTask"
-                        task_id="looping_pick_and_place_object"
-                        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-                        task="{task}"
-                      />
-                      <Action ID="SetupMTCCurrentState" task="{task}" />
-                      <Action
-                        ID="SetupMTCApproachGrasp"
-                        parameters="{parameters}"
-                        target_object="{cuboid_object}"
-                        monitored_stage="{monitored_stage}"
-                        task="{task}"
-                      />
-                      <Action
-                        ID="SetupMTCGenerateCuboidGrasps"
-                        parameters="{parameters}"
-                        target_object="{cuboid_object}"
-                        monitored_stage="{monitored_stage}"
-                        task="{task}"
-                      />
-                      <Action
-                        ID="SetupMTCRetractFromGrasp"
-                        parameters="{parameters}"
-                        target_object="{cuboid_object}"
-                        task="{task}"
-                      />
-                      <Action
-                        ID="PlanMTCTask"
-                        solution="{solution}"
-                        task="{task}"
-                      />
+                      <Action ID="InitializeMTCTask" task_id="looping_pick_and_place_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{task}"/>
+                      <Action ID="SetupMTCCurrentState" task="{task}"/>
+                      <Action ID="SetupMTCApproachGrasp" parameters="{parameters}" target_object="{cuboid_object}" monitored_stage="{monitored_stage}" task="{task}"/>
+                      <Action ID="SetupMTCGenerateCuboidGrasps" parameters="{parameters}" target_object="{cuboid_object}" monitored_stage="{monitored_stage}" task="{task}"/>
+                      <Action ID="SetupMTCRetractFromGrasp" parameters="{parameters}" target_object="{cuboid_object}" task="{task}"/>
+                      <Action ID="PlanMTCTask" solution="{solution}" task="{task}"/>
 
                       <!-- Execute the planned motion. -->
-                      <Action ID="ExecuteMTCTask" solution="{solution}" />
+                      <Action ID="ExecuteMTCTask" solution="{solution}"/>
                     </Control>
 
                     <!-- TryGraspInner fallback condition. Wait for a short time before triggering a retry. -->
                     <Control ID="Sequence">
-                      <Action ID="WaitForDuration" delay_duration="1" />
-                      <Action ID="AlwaysFailure" />
+                      <Action ID="WaitForDuration" delay_duration="1"/>
+                      <Action ID="AlwaysFailure"/>
                     </Control>
                   </Control>
                 </Decorator>
                 <!-- Move home. -->
-                <SubTree
-                  ID="Move to Waypoint"
-                  waypoint_name="Look at Pick and Place Zone"
-                  joint_group_name="manipulator"
-                  controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-                />
+                <SubTree ID="Move to Waypoint" waypoint_name="Look at Pick and Place Zone" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
 
                 <!-- Move to the current place waypoint set by IterateThroughPlaceWaypoints, then open the gripper. -->
-                <SubTree
-                  ID="Move to Waypoint"
-                  waypoint_name="{place_waypoint}"
-                  joint_group_name="manipulator"
-                  controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-                />
-                <Action
-                  ID="MoveGripperAction"
-                  gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-                  position="0.0"
-                />
+                <SubTree ID="Move to Waypoint" waypoint_name="{place_waypoint}" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
 
                 <!-- Finally, return home. -->
-                <SubTree
-                  ID="Move to Waypoint"
-                  waypoint_name="Look at Pick and Place Zone"
-                  joint_group_name="manipulator"
-                  controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-                />
+                <SubTree ID="Move to Waypoint" waypoint_name="Look at Pick and Place Zone" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
               </Control>
             </Decorator>
           </Decorator>
 
           <!-- TryGraspOuter fallback condition. Reset scene objects, move home, and then wait a short while before triggering a retry. -->
           <Control ID="Sequence">
-            <Action
-              ID="ResetPlanningSceneObjects"
-              apply_planning_scene_service="/apply_planning_scene"
-            />
-            <SubTree
-              ID="Move to Waypoint"
-              waypoint_name="Look at Pick and Place Zone"
-              joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            />
-            <Action ID="WaitForDuration" delay_duration="5" />
-            <Action ID="AlwaysSuccess" />
+            <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
+            <SubTree ID="Move to Waypoint" waypoint_name="Look at Pick and Place Zone" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <Action ID="WaitForDuration" delay_duration="5"/>
+            <Action ID="AlwaysSuccess"/>
           </Control>
         </Control>
       </Decorator>
@@ -203,8 +97,8 @@
   <TreeNodesModel>
     <SubTree ID="Looping Pick and Place Object">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Basic Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_cabinet_door.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_cabinet_door.xml
index 02e9b613..c08f0a7b 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_cabinet_door.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_cabinet_door.xml
@@ -1,178 +1,59 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <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="true"
-  >
+  <BehaviorTree ID="Open Cabinet Door" _description="Open a cabinet door by grasping and pulling its handle" _favorite="true">
     <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}"
-      />
+      <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" />
+      <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}"
-      />
+      <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}"
-      />
+      <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}"
-        />
+        <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}"
-      />
+      <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="ForEach" 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}"
-        />
+        <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"
-      />
+      <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="ForEach"
-        vector_in="{move_along_arc_subframes}"
-        out="{subframe}"
-      >
-        <Action
-          ID="AddSubframeToObject"
-          subframe="{subframe}"
-          graspable_object="{handle_object}"
-        />
+      <Decorator ID="ForEach" 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"
-      />
+      <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="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"
-          planner_interface="moveit_default"
-        />
+        <Action ID="SetupMTCMoveToNamedState" task="{open_door_fixed_handle_task}" planning_group_name="gripper" goal_state_name="open" planner_interface="moveit_default"/>
         <!--Approach Grasp Point-->
-        <Action
-          ID="SetupMTCApproachGrasp"
-          task="{open_door_fixed_handle_task}"
-          monitored_stage="{monitored_stage}"
-          target_object="{handle_object}"
-          parameters="{parameters}"
-        />
+        <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}"
-        />
+        <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" planner_interface="moveit_default"/>-->
         <!--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}"
-        />
+        <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"
-        />
+        <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}"
-          />
+          <Action ID="ExecuteMTCTask" solution="{open_door_fixed_handle_solution}"/>
           <!--THEN reset the controllers-->
-          <Action
-            ID="ActivateControllers"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          />
+          <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" />
+            <Action ID="ActivateControllers" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <Action ID="AlwaysFailure"/>
           </Control>
         </Control>
       </Control>
@@ -181,7 +62,7 @@
   <TreeNodesModel>
     <SubTree ID="Open Cabinet Door">
       <MetadataFields>
-        <Metadata subcategory="Perception - Machine Learning" />
+        <Metadata subcategory="Perception - Machine Learning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_door_fixed_handle_ml.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_door_fixed_handle_ml.xml
index b1c24367..0117ee1a 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_door_fixed_handle_ml.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_door_fixed_handle_ml.xml
@@ -1,175 +1,67 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Open Door Fixed Handle ML">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Open Door Fixed Handle ML"
-    _description="Attempts to open a door with a fixed handle (such as cabinet doors) using machine learning."
-    _favorite="false"
-    _hardcoded="true"
-  >
+  <BehaviorTree ID="Open Door Fixed Handle ML" _description="Attempts to open a door with a fixed handle (such as cabinet doors) using machine learning." _favorite="false" _hardcoded="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Load the parameters needed for this Objective -->
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="open_door_fixed_handle_MTC_config.yaml"
-        parameters="{parameters}"
-      />
+      <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" />
+      <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
+      <SubTree ID="Update Admittance Controller" _collapsed="true"/>
       <!-- Look at the desired location where objects (door) will be present -->
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Extended Right"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
-      <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="space booth cabinet door"
-      />
+      <SubTree ID="Move to Waypoint" waypoint_name="Extended Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <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="space booth cabinet door"/>
       <!-- Get the masks for the handle subframe of the door GraspableObject -->
       <!-- May require multiple view-points due to incomplete point clouds due to angle. -->
       <Control ID="Fallback">
-        <SubTree
-          ID="Detect Fixed Handle Graspable Object"
-          _collapsed="true"
-          grasp_pose="{grasp_pose}"
-        />
+        <SubTree ID="Detect Fixed Handle Graspable Object" _collapsed="true" grasp_pose="{grasp_pose}"/>
         <Control ID="Sequence">
-          <SubTree
-            ID="Move to Waypoint"
-            waypoint_name="Extended Right 2"
-            joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          />
-          <SubTree
-            ID="Detect Fixed Handle Graspable Object"
-            _collapsed="true"
-            grasp_pose="{grasp_pose}"
-          />
+          <SubTree ID="Move to Waypoint" waypoint_name="Extended Right 2" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+          <SubTree ID="Detect Fixed Handle Graspable Object" _collapsed="true" grasp_pose="{grasp_pose}"/>
         </Control>
         <Control ID="Sequence">
-          <SubTree
-            ID="Move to Waypoint"
-            waypoint_name="Extended Right"
-            joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          />
-          <SubTree
-            ID="Detect Fixed Handle Graspable Object"
-            _collapsed="true"
-            grasp_pose="{grasp_pose}"
-          />
+          <SubTree ID="Move to Waypoint" waypoint_name="Extended Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+          <SubTree ID="Detect Fixed Handle Graspable Object" _collapsed="true" grasp_pose="{grasp_pose}"/>
         </Control>
       </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"
-      />
-      <SubTree
-        ID="Detect Hinge Line Segment"
-        _collapsed="true"
-        line_segment_poses="{line_segment_poses}"
-      />
+      <SubTree ID="Move to Waypoint" waypoint_name="Hinge View" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <SubTree ID="Detect Hinge Line Segment" _collapsed="true" line_segment_poses="{line_segment_poses}"/>
       <!-- 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_poses="{line_segment_poses}" move_along_arc_subframes="{move_along_arc_subframes}"/>
       <!-- Add the subframes to the Object-->
-      <Decorator
-        ID="ForEach"
-        vector_in="{move_along_arc_subframes}"
-        out="{subframe}"
-      >
-        <Action
-          ID="AddSubframeToObject"
-          subframe="{subframe}"
-          graspable_object="{door_object}"
-        />
+      <Decorator ID="ForEach" vector_in="{move_along_arc_subframes}" out="{subframe}">
+        <Action ID="AddSubframeToObject" subframe="{subframe}" graspable_object="{door_object}"/>
       </Decorator>
       <!-- Open Gripper -->
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.0"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
       <!-- Plan and optionally Execute MTC task -->
       <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}"
-        />
+        <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"
-          planner_interface="moveit_default"
-        />
+        <Action ID="SetupMTCMoveToNamedState" task="{open_door_fixed_handle_task}" planning_group_name="gripper" goal_state_name="open" planner_interface="moveit_default"/>
         <!--Approach Grasp Point-->
-        <Action
-          ID="SetupMTCApproachGrasp"
-          task="{open_door_fixed_handle_task}"
-          monitored_stage="{monitored_stage}"
-          target_object="{handle_object}"
-          parameters="{parameters}"
-        />
+        <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}"
-        />
+        <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" planner_interface="moveit_default"/>-->
         <!--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}"
-        />
+        <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"
-          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}"
-        />
+        <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}"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Open Door Fixed Handle ML">
       <MetadataFields>
-        <Metadata subcategory="Perception - Machine Learning" />
+        <Metadata subcategory="Perception - Machine Learning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_door_lever_handle_ml.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_door_lever_handle_ml.xml
index 2377e17d..08c80b9b 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_door_lever_handle_ml.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_door_lever_handle_ml.xml
@@ -1,113 +1,39 @@
-<?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"
-  >
+  <BehaviorTree ID="Open Door Lever Handle ML" _description="Attempts to open a door with a lever handle using machine learning." _favorite="false" _hardcoded="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Load the parameters needed for this Objective -->
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="open_door_lever_handle_MTC_config.yaml"
-        parameters="{parameters}"
-      />
+      <Action ID="LoadObjectiveParameters" config_file_name="open_door_lever_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" />
+      <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
+      <SubTree ID="Update Admittance Controller" _collapsed="true"/>
       <!-- Look at the desired location where objects (door) will be present -->
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Extended Right 2"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
-      <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}"
-      />
-      <Action
-        ID="GetGraspAndTwistSubframes"
-        grasp_rotation_z_radians="1.5708"
-        target_grasp_pose="{grasp_pose}"
-        grasp_and_twist_subframes="{grasp_and_twist_subframes}"
-      />
-      <Decorator
-        ID="ForEach"
-        vector_in="{grasp_and_twist_subframes}"
-        out="{subframe}"
-      >
-        <Action
-          ID="AddSubframeToObject"
-          subframe="{subframe}"
-          graspable_object="{door_object}"
-        />
+      <SubTree ID="Move to Waypoint" waypoint_name="Extended Right 2" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <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}"/>
+      <Action ID="GetGraspAndTwistSubframes" grasp_rotation_z_radians="1.5708" target_grasp_pose="{grasp_pose}" grasp_and_twist_subframes="{grasp_and_twist_subframes}"/>
+      <Decorator ID="ForEach" vector_in="{grasp_and_twist_subframes}" out="{subframe}">
+        <Action ID="AddSubframeToObject" subframe="{subframe}" graspable_object="{door_object}"/>
       </Decorator>
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.0"
-      />
+      <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}"
-        />
-        <Action
-          ID="SetupMTCCurrentState"
-          task="{open_door_lever_handle_task}"
-        />
-        <Action
-          ID="SetupMTCGraspAndTwistThenMoveAlongArcPush"
-          graspable_object="{door_object}"
-          parameters="{parameters}"
-          task="{open_door_lever_handle_task}"
-          handle_length="{handle_length}"
-          handle_z_offset="{handle_z_offset}"
-        />
-        <Action
-          ID="PlanMTCTask"
-          task="{open_door_lever_handle_task}"
-          solution="{open_door_lever_handle_solution}"
-        />
-        <SubTree
-          ID="Wait for Trajectory Approval if User Available"
-          solution="{open_door_lever_handle_solution}"
-        />
-        <Action
-          ID="ExecuteMTCTask"
-          solution="{open_door_lever_handle_solution}"
-        />
+        <Action ID="InitializeMTCTask" task_id="open_door_lever_handle_ml" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{open_door_lever_handle_task}"/>
+        <Action ID="SetupMTCCurrentState" task="{open_door_lever_handle_task}"/>
+        <Action ID="SetupMTCGraspAndTwistThenMoveAlongArcPush" graspable_object="{door_object}" parameters="{parameters}" task="{open_door_lever_handle_task}" handle_length="{handle_length}" handle_z_offset="{handle_z_offset}"/>
+        <Action ID="PlanMTCTask" task="{open_door_lever_handle_task}" solution="{open_door_lever_handle_solution}"/>
+        <SubTree ID="Wait for Trajectory Approval if User Available" solution="{open_door_lever_handle_solution}"/>
+        <Action ID="ExecuteMTCTask" solution="{open_door_lever_handle_solution}"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Open Door Lever Handle">
       <MetadataFields>
-        <Metadata subcategory="Perception - Machine Learning" />
+        <Metadata subcategory="Perception - Machine Learning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_lever_handle_door.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_lever_handle_door.xml
index 945db372..fad507bf 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_lever_handle_door.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/open_lever_handle_door.xml
@@ -1,149 +1,50 @@
-<?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"
-  >
+  <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="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="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}"
-      />
+      <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="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="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-->
-      <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"
-        generate_front_face="true"
-        object_id="lever_door"
-        dy="0.100000"
-        dx="0.100000"
-        dz="0.100000"
-        pose="{handle_pose}"
-      />
+      <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" generate_front_face="true" object_id="lever_door" dy="0.100000" dx="0.100000" dz="0.100000" pose="{handle_pose}"/>
       <!--Add the subframes to the Object-->
-      <Decorator
-        ID="ForEach"
-        vector_in="{grasp_and_twist_subframes}"
-        out="{subframe}"
-      >
-        <Action
-          ID="AddSubframeToObject"
-          subframe="{subframe}"
-          graspable_object="{door_object}"
-        />
+      <Decorator ID="ForEach" vector_in="{grasp_and_twist_subframes}" out="{subframe}">
+        <Action ID="AddSubframeToObject" subframe="{subframe}" graspable_object="{door_object}"/>
       </Decorator>
-      <SubTree ID="Close Gripper" _collapsed="true" />
+      <SubTree ID="Close Gripper" _collapsed="true"/>
       <Control ID="Sequence" name="OpenLeverHandleDoor">
-        <Action
-          ID="InitializeMTCTask"
-          task_id="open_lever_handle_door"
-          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          task="{open_lever_handle_door_task}"
-        />
-        <Action
-          ID="SetupMTCCurrentState"
-          task="{open_lever_handle_door_task}"
-        />
-        <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}"
-        />
+        <Action ID="InitializeMTCTask" task_id="open_lever_handle_door" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{open_lever_handle_door_task}"/>
+        <Action ID="SetupMTCCurrentState" task="{open_lever_handle_door_task}"/>
+        <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.-->
         <Control ID="IfThenElse" name="execute_and_reset_controllers">
           <!--IF task execution succeeds-->
-          <Action
-            ID="ExecuteMTCTask"
-            solution="{open_lever_handle_door_solution}"
-          />
+          <Action ID="ExecuteMTCTask" solution="{open_lever_handle_door_solution}"/>
           <!--THEN reset the controllers-->
-          <Action
-            ID="ActivateControllers"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          />
+          <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" />
+            <Action ID="ActivateControllers" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <Action ID="AlwaysFailure"/>
           </Control>
         </Control>
       </Control>
-      <SubTree ID="Close Gripper" _collapsed="true" />
+      <SubTree ID="Close Gripper" _collapsed="true"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Open Lever Handle Door">
       <MetadataFields>
-        <Metadata subcategory="Perception - Machine Learning" />
+        <Metadata subcategory="Perception - Machine Learning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/pick_apriltag_labeled_object.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/pick_apriltag_labeled_object.xml
index 29725c8c..fb8791fc 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/pick_apriltag_labeled_object.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/pick_apriltag_labeled_object.xml
@@ -1,57 +1,25 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Pick April Tag Labeled Object"
-    _description="Picks up an object that is at a known pose offset from an AprilTag marker."
-    _favorite="false"
-  >
+  <BehaviorTree ID="Pick April Tag Labeled Object" _description="Picks up an object that is at a known pose offset from an AprilTag marker." _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <Control ID="Sequence" name="Setup">
-        <SubTree ID="Open Gripper" />
-        <SubTree
-          ID="Move to Waypoint"
-          waypoint_name="Look at April Tag"
-          joint_group_name="manipulator"
-          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        />
+        <SubTree ID="Open Gripper"/>
+        <SubTree ID="Move to Waypoint" waypoint_name="Look at April Tag" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
       </Control>
-      <SubTree
-        ID="Sample April Tag"
-        num_samples="5"
-        tag_id="1"
-        apriltag_config="51mm_apriltag_detection_config.yaml"
-        max_distance="0.02"
-        max_rotation="0.2"
-        avg_pose="{tag_pose}"
-      />
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="apriltag1_grasp_offset.yaml"
-        parameters="{pose_parameters}"
-      />
+      <SubTree ID="Sample April Tag" num_samples="5" tag_id="1" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+      <Action ID="LoadObjectiveParameters" config_file_name="apriltag1_grasp_offset.yaml" parameters="{pose_parameters}"/>
       <Control ID="Sequence" name="GraspObject">
-        <Action
-          ID="TransformPoseFrame"
-          input_pose="{tag_pose}"
-          target_frame_id="world"
-          output_pose="{tag_pose_world}"
-        />
-        <Action
-          ID="TransformPoseFromYaml"
-          input_pose="{tag_pose_world}"
-          pose_parameters="{pose_parameters}"
-          parameter_namespace="GraspOffset"
-          output_pose="{grasp_pose}"
-        />
-        <SubTree ID="Pick Object" grasp_pose="{grasp_pose}" />
+        <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
+        <Action ID="TransformPoseFromYaml" input_pose="{tag_pose_world}" pose_parameters="{pose_parameters}" parameter_namespace="GraspOffset" output_pose="{grasp_pose}"/>
+        <SubTree ID="Pick Object" grasp_pose="{grasp_pose}"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Pick April Tag Labeled Object">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
+        <Metadata subcategory="Application - Basic Examples"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/pick_object.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/pick_object.xml
index daff1deb..3758d491 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/pick_object.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/pick_object.xml
@@ -1,70 +1,23 @@
-<?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"
-  >
+  <BehaviorTree ID="Pick Object" _description="Pick up and lift a small object" _favorite="true">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="pick_object_config.yaml"
-        parameters="{parameters}"
-      />
-      <SubTree ID="Open Gripper" />
+      <Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml" parameters="{parameters}"/>
+      <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}"
-        />
+        <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>
       <Decorator ID="ForEach" 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}" />
+          <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>
       </Decorator>
     </Control>
@@ -72,8 +25,8 @@
   <TreeNodesModel>
     <SubTree ID="Pick Object">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Basic Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/pick_place_object.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/pick_place_object.xml
index 29cd38f2..56efa0ff 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/pick_place_object.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/pick_place_object.xml
@@ -1,23 +1,11 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick and Place Object">
-  <BehaviorTree
-    ID="Pick and Place Object"
-    _description="Pick up a small object and place it at a known location"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Pick and Place Object" _description="Pick up a small object and place it at a known location" _favorite="true">
     <Control ID="Sequence">
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="pick_place_object_config.yaml"
-        parameters="{parameters}"
-      />
-      <Script code="first_attempt:=true" />
+      <Action ID="LoadObjectiveParameters" config_file_name="pick_place_object_config.yaml" parameters="{parameters}"/>
+      <Script code="first_attempt:=true"/>
       <!-- Open the gripper before doing any planning. -->
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.0"
-      />
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
       <Control ID="IfThenElse" name="TryGraspOuter">
         <!-- This decorator contains the logic to detect an object and then execute motion to grasp it.
                      If grasping is aborted because the object changed while the robot was moving towards it, we will retry up to 5 times before giving up. -->
@@ -25,265 +13,97 @@
           <Control ID="IfThenElse" name="TryGraspInner">
             <Control ID="Sequence">
               <!-- Clear the collision octree. -->
-              <Action ID="ClearSnapshot" />
+              <Action ID="ClearSnapshot"/>
               <!-- Remove any objects which might remain in the planning scene from previous pick attempts -->
-              <Action
-                ID="ResetPlanningSceneObjects"
-                apply_planning_scene_service="apply_planning_scene"
-              />
+              <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="apply_planning_scene"/>
               <!-- Get the latest point cloud from the target topic-->
-              <Action
-                ID="GetPointCloud"
-                topic_name="/wrist_mounted_camera/depth/color/points"
-                message_out="{point_cloud}"
-              />
+              <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
               <!-- Publish point cloud for display by UI -->
-              <Action
-                ID="SendPointCloudToUI"
-                point_cloud="{point_cloud}"
-                pcd_topic="/pcd_pointcloud_captures"
-              />
+              <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
               <!-- Detect all cuboids in that point cloud-->
-              <Action
-                ID="FindSingularCuboids"
-                point_cloud="{point_cloud}"
-                parameters="{parameters}"
-                detected_shapes="{cuboids}"
-              />
+              <Action ID="FindSingularCuboids" point_cloud="{point_cloud}" parameters="{parameters}" detected_shapes="{cuboids}"/>
               <!-- Find the closest cuboid to the pose calculated from the user's original click -->
-              <Action
-                ID="GetPointsFromUser"
-                view_name="/wrist_mounted_camera/color/image_raw"
-                point_names="grasp_pose"
-                point_prompts="Click on the surface you wish to place the object on"
-                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}"
-              />
-              <Decorator
-                ID="ForEach"
-                vector_in="{adjusted_poses}"
-                out="{grasp_pose}"
-              >
+              <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Click on the surface you wish to place the object on" 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}"/>
+              <Decorator ID="ForEach" vector_in="{adjusted_poses}" out="{grasp_pose}">
                 <Control ID="Sequence">
-                  <Action
-                    ID="GetClosestObjectToPose"
-                    objects="{cuboids}"
-                    pose="{grasp_pose}"
-                    distance_threshold="0.3"
-                    closest_object="{cuboid_object}"
-                  />
+                  <Action ID="GetClosestObjectToPose" objects="{cuboids}" pose="{grasp_pose}" distance_threshold="0.3" closest_object="{cuboid_object}"/>
                   <!-- Add this cuboid to the planning scene. -->
-                  <Action
-                    ID="ModifyObjectInPlanningScene"
-                    object="{cuboid_object}"
-                    apply_planning_scene_service="/apply_planning_scene"
-                  />
+                  <Action ID="ModifyObjectInPlanningScene" object="{cuboid_object}" apply_planning_scene_service="/apply_planning_scene"/>
                   <!-- Use point cloud to update the collision octomap (happens after ModifyObjectInPlanningScene so the cuboid is excluded from the octomap)-->
-                  <Action
-                    ID="UpdatePlanningSceneService"
-                    point_cloud="{point_cloud}"
-                    point_cloud_service="/point_cloud_service"
-                  />
+                  <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
                   <!-- Set up an MTC task to pick the cuboid and then plan the task. -->
-                  <Action
-                    ID="InitializeMTCTask"
-                    task_id="pick_place_object"
-                    controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-                    task="{task}"
-                  />
-                  <Action ID="SetupMTCCurrentState" task="{task}" />
-                  <Action
-                    ID="SetupMTCApproachGrasp"
-                    parameters="{parameters}"
-                    target_object="{cuboid_object}"
-                    monitored_stage="{monitored_stage}"
-                    task="{task}"
-                  />
-                  <Action
-                    ID="SetupMTCGenerateCuboidGrasps"
-                    parameters="{parameters}"
-                    target_object="{cuboid_object}"
-                    monitored_stage="{monitored_stage}"
-                    task="{task}"
-                  />
-                  <Action
-                    ID="SetupMTCRetractFromGrasp"
-                    parameters="{parameters}"
-                    target_object="{cuboid_object}"
-                    task="{task}"
-                  />
-                  <Action
-                    ID="PlanMTCTask"
-                    solution="{solution}"
-                    task="{task}"
-                  />
+                  <Action ID="InitializeMTCTask" task_id="pick_place_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{task}"/>
+                  <Action ID="SetupMTCCurrentState" task="{task}"/>
+                  <Action ID="SetupMTCApproachGrasp" parameters="{parameters}" target_object="{cuboid_object}" monitored_stage="{monitored_stage}" task="{task}"/>
+                  <Action ID="SetupMTCGenerateCuboidGrasps" parameters="{parameters}" target_object="{cuboid_object}" monitored_stage="{monitored_stage}" task="{task}"/>
+                  <Action ID="SetupMTCRetractFromGrasp" parameters="{parameters}" target_object="{cuboid_object}" task="{task}"/>
+                  <Action ID="PlanMTCTask" solution="{solution}" task="{task}"/>
                   <!-- Send trajectory preview to user only if this is the first time trying to pick the object. -->
                   <Precondition if="first_attempt" else="SUCCESS">
-                    <SubTree
-                      ID="Wait for Trajectory Approval if User Available"
-                      solution="{solution}"
-                    />
+                    <SubTree ID="Wait for Trajectory Approval if User Available" solution="{solution}"/>
                   </Precondition>
                   <!-- Set value to port to skip preview on following attempts. -->
-                  <Script code="first_attempt:=false" />
+                  <Script code="first_attempt:=false"/>
                   <!-- Split the solution between the big freespace move to the pre-grasp pose and the short Cartesian move to grasp. -->
-                  <Action
-                    ID="SplitMTCSolution"
-                    solution_in="{solution}"
-                    index="3"
-                    solution_out_1="{solution_approach}"
-                    solution_out_2="{solution_grasp}"
-                  />
+                  <Action ID="SplitMTCSolution" solution_in="{solution}" index="3" solution_out_1="{solution_approach}" solution_out_2="{solution_grasp}"/>
                   <!-- Do the following in parallel: -->
                   <Control ID="Parallel" success_count="1" failure_count="1">
                     <!-- Parallel branch 1: Execute the part of the solution that moves to the pre-grasp pose. -->
-                    <Action
-                      ID="ExecuteMTCTask"
-                      solution="{solution_approach}"
-                    />
+                    <Action ID="ExecuteMTCTask" solution="{solution_approach}"/>
                     <!-- Parallel branch 2: Continuously analyze point clouds to make sure we can still find an object that is similar to and located at nearly the same pose as the original object we planned to grasp. -->
                     <Decorator ID="KeepRunningUntilFailure">
                       <Control ID="IfThenElse" name="IfObjectVisible">
                         <!-- IF -->
-                        <Action
-                          ID="IsConstraintSatisfied"
-                          object="{cuboid_object}"
-                          parameters="{parameters}"
-                        />
+                        <Action ID="IsConstraintSatisfied" object="{cuboid_object}" parameters="{parameters}"/>
                         <!-- THEN -->
                         <Control ID="Sequence">
-                          <Action
-                            ID="GetPointCloud"
-                            topic_name="/wrist_mounted_camera/depth/color/points"
-                            message_out="{point_cloud}"
-                          />
-                          <Action
-                            ID="FindSingularCuboids"
-                            parameters="{parameters}"
-                            point_cloud="{point_cloud}"
-                            detected_shapes="{cuboids}"
-                          />
-                          <Action
-                            ID="GetPointsFromUser"
-                            view_name="/wrist_mounted_camera/color/image_raw"
-                            point_names="grasp_pose"
-                            point_prompts="Click on the object you would like 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}"
-                          />
-                          <Decorator
-                            ID="ForEach"
-                            vector_in="{adjusted_poses}"
-                            out="{grasp_pose}"
-                          >
+                          <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
+                          <Action ID="FindSingularCuboids" parameters="{parameters}" point_cloud="{point_cloud}" detected_shapes="{cuboids}"/>
+                          <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Click on the object you would like 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}"/>
+                          <Decorator ID="ForEach" vector_in="{adjusted_poses}" out="{grasp_pose}">
                             <Control ID="Sequence">
-                              <Action
-                                ID="GetClosestObjectToPose"
-                                objects="{cuboids}"
-                                pose="{grasp_pose}"
-                                distance_threshold="0.3"
-                                closest_object="{new_cuboid_object}"
-                              />
-                              <Action
-                                ID="CheckCuboidSimilarity"
-                                input_cuboid="{new_cuboid_object}"
-                                reference_cuboid="{cuboid_object}"
-                                base_frame="world"
-                                distance_threshold="0.04"
-                                orientation_threshold="3.14"
-                              />
+                              <Action ID="GetClosestObjectToPose" objects="{cuboids}" pose="{grasp_pose}" distance_threshold="0.3" closest_object="{new_cuboid_object}"/>
+                              <Action ID="CheckCuboidSimilarity" input_cuboid="{new_cuboid_object}" reference_cuboid="{cuboid_object}" base_frame="world" distance_threshold="0.04" orientation_threshold="3.14"/>
                             </Control>
                           </Decorator>
                         </Control>
                         <!-- ELSE -->
-                        <Action ID="AlwaysSuccess" />
+                        <Action ID="AlwaysSuccess"/>
                       </Control>
                     </Decorator>
                   </Control>
                   <!-- We will reach this part of the Objective if the move to the pre-grasp pose succeeds without the system detecting a significant change in the target object. -->
-                  <Action ID="ExecuteMTCTask" solution="{solution_grasp}" />
+                  <Action ID="ExecuteMTCTask" solution="{solution_grasp}"/>
                 </Control>
               </Decorator>
             </Control>
             <!-- The "Then" condition of the TryGraspInner IfThenElse control. If motion execution succeeded, emit a success to exit the retry loop. -->
-            <Action ID="AlwaysSuccess" />
+            <Action ID="AlwaysSuccess"/>
             <!-- The "Else" condition of the TryGraspInner IfThenElse control.  We will reach this part of the Objective if the system detects a significant change in the target object. -->
             <Control ID="Sequence">
               <!-- Pause for a short duration, then emit a failure to trigger a retry. -->
-              <Action ID="WaitForDuration" delay_duration="1" />
-              <Action ID="AlwaysFailure" />
+              <Action ID="WaitForDuration" delay_duration="1"/>
+              <Action ID="AlwaysFailure"/>
             </Control>
           </Control>
         </Decorator>
         <!-- The "Then" condition of the TryGraspOuter IfThenElse control. After successful grasp, move to dropoff pose and release object, then return to the Look at Pick and Place Zone pose. -->
         <Control ID="Sequence">
-          <SubTree
-            ID="Move to Waypoint"
-            waypoint_name="Place"
-            joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          />
-          <Action
-            ID="MoveGripperAction"
-            gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-            position="0.0"
-          />
-          <SubTree
-            ID="Move to Waypoint"
-            waypoint_name="Look at Pick and Place Zone"
-            joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          />
+          <SubTree ID="Move to Waypoint" waypoint_name="Place" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+          <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
+          <SubTree ID="Move to Waypoint" waypoint_name="Look at Pick and Place Zone" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
         </Control>
         <!-- The "Else" condition of the TryGraspOuter IfThenElse control. Move to the Look at Pick and Place Zone pose if all retry attempts failed. -->
         <Control ID="Sequence">
-          <Action
-            ID="ResetPlanningSceneObjects"
-            apply_planning_scene_service="/apply_planning_scene"
-          />
-          <SubTree
-            ID="Move to Waypoint"
-            waypoint_name="Look at Pick and Place Zone"
-            joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-          />
-          <Action ID="AlwaysFailure" />
+          <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
+          <SubTree ID="Move to Waypoint" waypoint_name="Look at Pick and Place Zone" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+          <Action ID="AlwaysFailure"/>
         </Control>
       </Control>
     </Control>
@@ -291,8 +111,8 @@
   <TreeNodesModel>
     <SubTree ID="Pick and Place Object">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Basic Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/push_button.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/push_button.xml
index e1a42c0f..caf7d6cc 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/push_button.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/push_button.xml
@@ -1,127 +1,43 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Push Button">
-  <BehaviorTree
-    ID="Push Button"
-    _description="Push a button with the end effector"
-  >
+  <BehaviorTree ID="Push Button" _description="Push a button with the end effector">
     <Control ID="Sequence">
-      <SubTree ID="Re-Zero Force-Torque Sensors" />
-      <SubTree ID="Close Gripper" />
-      <Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}" />
+      <SubTree ID="Re-Zero Force-Torque Sensors"/>
+      <SubTree ID="Close Gripper"/>
+      <Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}"/>
       <Control ID="Sequence">
-        <Action
-          ID="InitializeMTCTask"
-          task_id="push_button"
-          controller_names="/joint_trajectory_controller"
-          task="{push_button_task}"
-        />
-        <Action ID="SetupMTCCurrentState" task="{push_button_task}" />
-        <Action
-          ID="GetPointsFromUser"
-          view_name="/wrist_mounted_camera/color/image_raw"
-          point_names="target_pose"
-          point_prompts="Click on the button you want to press"
-          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="InitializeMTCTask" task_id="push_button" controller_names="/joint_trajectory_controller" task="{push_button_task}"/>
+        <Action ID="SetupMTCCurrentState" task="{push_button_task}"/>
+        <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="target_pose" point_prompts="Click on the button you want to press" 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}"/>
         <Decorator ID="ForEach" vector_in="{output_poses}" out="{pose}">
           <Control ID="Sequence">
-            <Action
-              ID="TransformPose"
-              output_pose="{target_pose}"
-              quaternion_xyzw="0.7071;0.7071;0;0"
-              translation_xyz="0;0;0.1"
-              input_pose="{pose}"
-            />
-            <Action
-              ID="SetupMTCPlanToPose"
-              ik_frame="grasp_link"
-              planning_group_name="manipulator"
-              target_pose="{target_pose}"
-              task="{push_button_task}"
-            />
+            <Action ID="TransformPose" output_pose="{target_pose}" quaternion_xyzw="0.7071;0.7071;0;0" translation_xyz="0;0;0.1" input_pose="{pose}"/>
+            <Action ID="SetupMTCPlanToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{push_button_task}"/>
           </Control>
         </Decorator>
-        <Action
-          ID="SetupMTCUpdateGroupCollisionRule"
-          name="AllowGripperCollisionWithOctomap"
-          group_name="gripper"
-          object_name="&lt;octomap&gt;"
-          allow_collision="true"
-          task="{push_button_task}"
-        />
-        <Action
-          ID="SetupMTCMoveAlongFrameAxis"
-          task="{push_button_task}"
-          hand_frame="grasp_link"
-          axis_frame="grasp_link"
-          axis_x="0.0"
-          axis_y="0.0"
-          axis_z="1.0"
-          max_distance="0.2"
-          min_distance="0.05"
-          planning_group_name="manipulator"
-          velocity_scale="0.05"
-          acceleration_scale="0.05"
-        />
-        <Action
-          ID="SetupMTCUpdateGroupCollisionRule"
-          name="ForbidGripperCollisionWithOctomap"
-          group_name="gripper"
-          object_name="&lt;octomap&gt;"
-          allow_collision="false"
-          task="{push_button_task}"
-        />
-        <Action
-          ID="PlanMTCTask"
-          solution="{full_push_button_solution}"
-          task="{push_button_task}"
-        />
-        <Action
-          ID="SplitMTCSolution"
-          solution_in="{full_push_button_solution}"
-          index="3"
-          solution_out_1="{move_to_approach_solution}"
-          solution_out_2="{push_solution}"
-        />
-        <Action
-          ID="WaitForUserTrajectoryApproval"
-          solution="{full_push_button_solution}"
-        />
-        <Action ID="ExecuteMTCTask" solution="{move_to_approach_solution}" />
-        <Action
-          ID="SaveCurrentState"
-          saved_robot_state="{pre_approach_robot_state}"
-        />
+        <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" group_name="gripper" object_name="&lt;octomap&gt;" allow_collision="true" task="{push_button_task}"/>
+        <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_button_task}" hand_frame="grasp_link" axis_frame="grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05"/>
+        <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" group_name="gripper" object_name="&lt;octomap&gt;" allow_collision="false" task="{push_button_task}"/>
+        <Action ID="PlanMTCTask" solution="{full_push_button_solution}" task="{push_button_task}"/>
+        <Action ID="SplitMTCSolution" solution_in="{full_push_button_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}"/>
+        <Action ID="WaitForUserTrajectoryApproval" solution="{full_push_button_solution}"/>
+        <Action ID="ExecuteMTCTask" solution="{move_to_approach_solution}"/>
+        <Action ID="SaveCurrentState" saved_robot_state="{pre_approach_robot_state}"/>
         <Control ID="Parallel" success_count="1" failure_count="1">
-          <SubTree ID="Force Exceeds Threshold" />
-          <Action ID="ExecuteMTCTask" solution="{push_solution}" />
+          <SubTree ID="Force Exceeds Threshold"/>
+          <Action ID="ExecuteMTCTask" solution="{push_solution}"/>
         </Control>
       </Control>
-      <SubTree
-        ID="Retreat To Initial Pose"
-        pre_approach_robot_state="{pre_approach_robot_state}"
-        initial_robot_state="{initial_robot_state}"
-      />
+      <SubTree ID="Retreat To Initial Pose" pre_approach_robot_state="{pre_approach_robot_state}" initial_robot_state="{initial_robot_state}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Push Button">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Basic Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/push_button_ml.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/push_button_ml.xml
index e36d0b5d..05646d74 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/push_button_ml.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/push_button_ml.xml
@@ -1,179 +1,56 @@
-<?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"
-  >
+  <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">
-      <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true" />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="Hinge View"
-        joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-      />
-      <SubTree ID="Clear Snapshot" _collapsed="true" />
-      <SubTree ID="Reset Planning Scene" _collapsed="true" />
-      <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"
-      />
-      <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="GetMasks3DFromMasks2D"
-        point_cloud="{point_cloud}"
-        masks2d="{masks2d}"
-        camera_info="{camera_info}"
-        masks3d="{masks3d}"
-      />
+      <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
+      <SubTree ID="Move to Waypoint" waypoint_name="Hinge View" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+      <SubTree ID="Clear Snapshot" _collapsed="true"/>
+      <SubTree ID="Reset Planning Scene" _collapsed="true"/>
+      <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"/>
+      <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="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
       <Decorator ID="ForEach" 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}"
-            pcd_topic="/pcd_pointcloud_captures"
-          />
+          <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
+          <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" pcd_topic="/pcd_pointcloud_captures"/>
         </Control>
       </Decorator>
-      <Action
-        ID="GetGraspableObjectsFromMasks3D"
-        point_cloud="{point_cloud}"
-        masks3d="{masks3d}"
-        base_frame="world"
-        plane_inlier_threshold="0.02"
-        minimum_face_area="0.000625"
-        face_separation_threshold="0.03"
-        graspable_objects="{graspable_objects}"
-      />
+      <Action ID="GetGraspableObjectsFromMasks3D" point_cloud="{point_cloud}" masks3d="{masks3d}" base_frame="world" plane_inlier_threshold="0.02" minimum_face_area="0.000625" face_separation_threshold="0.03" graspable_objects="{graspable_objects}"/>
       <Decorator ID="ForceSuccess">
         <Decorator ID="ForEach" vector_in="{graspable_objects}" out="{button}">
-          <Action ID="AlwaysFailure" />
+          <Action ID="AlwaysFailure"/>
         </Decorator>
       </Decorator>
-      <Action
-        ID="ExtractGraspableObjectPose"
-        graspable_object="{button}"
-        pose="{button_pose}"
-      />
-      <SubTree ID="Close Gripper" _collapsed="true" />
-      <Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}" />
+      <Action ID="ExtractGraspableObjectPose" graspable_object="{button}" pose="{button_pose}"/>
+      <SubTree ID="Close Gripper" _collapsed="true"/>
+      <Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}"/>
       <Control ID="Sequence">
-        <Action
-          ID="InitializeMTCTask"
-          task_id="push_button_ml"
-          controller_names="/joint_trajectory_controller"
-          task="{push_along_axis_task}"
-        />
-        <Action ID="SetupMTCCurrentState" task="{push_along_axis_task}" />
-        <Action
-          ID="SetupMTCPlanToPose"
-          ik_frame="grasp_link"
-          planning_group_name="manipulator"
-          target_pose="{button_pose}"
-          task="{push_along_axis_task}"
-        />
-        <Action
-          ID="SetupMTCUpdateGroupCollisionRule"
-          name="AllowGripperCollisionWithOctomap"
-          group_name="gripper"
-          object_name="&lt;octomap&gt;"
-          allow_collision="true"
-          task="{push_along_axis_task}"
-        />
-        <Action
-          ID="SetupMTCMoveAlongFrameAxis"
-          task="{push_along_axis_task}"
-          hand_frame="grasp_link"
-          axis_frame="grasp_link"
-          axis_x="0.0"
-          axis_y="0.0"
-          axis_z="1.0"
-          max_distance="0.2"
-          min_distance="0.05"
-          planning_group_name="manipulator"
-          velocity_scale="0.05"
-          acceleration_scale="0.05"
-        />
-        <Action
-          ID="SetupMTCUpdateGroupCollisionRule"
-          name="ForbidGripperCollisionWithOctomap"
-          group_name="gripper"
-          object_name="&lt;octomap&gt;"
-          allow_collision="false"
-          task="{push_along_axis_task}"
-        />
-        <Action
-          ID="PlanMTCTask"
-          solution="{full_push_along_axis_solution}"
-          task="{push_along_axis_task}"
-        />
-        <Action
-          ID="SplitMTCSolution"
-          solution_in="{full_push_along_axis_solution}"
-          index="3"
-          solution_out_1="{move_to_approach_solution}"
-          solution_out_2="{push_solution}"
-        />
-        <Action
-          ID="WaitForUserTrajectoryApproval"
-          solution="{full_push_along_axis_solution}"
-        />
-        <Action ID="ExecuteMTCTask" solution="{move_to_approach_solution}" />
-        <Action
-          ID="SaveCurrentState"
-          saved_robot_state="{pre_approach_robot_state}"
-        />
+        <Action ID="InitializeMTCTask" task_id="push_button_ml" controller_names="/joint_trajectory_controller" task="{push_along_axis_task}"/>
+        <Action ID="SetupMTCCurrentState" task="{push_along_axis_task}"/>
+        <Action ID="SetupMTCPlanToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{button_pose}" task="{push_along_axis_task}"/>
+        <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" group_name="gripper" object_name="&lt;octomap&gt;" allow_collision="true" task="{push_along_axis_task}"/>
+        <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="grasp_link" axis_frame="grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05"/>
+        <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" group_name="gripper" object_name="&lt;octomap&gt;" allow_collision="false" task="{push_along_axis_task}"/>
+        <Action ID="PlanMTCTask" solution="{full_push_along_axis_solution}" task="{push_along_axis_task}"/>
+        <Action ID="SplitMTCSolution" solution_in="{full_push_along_axis_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}"/>
+        <Action ID="WaitForUserTrajectoryApproval" solution="{full_push_along_axis_solution}"/>
+        <Action ID="ExecuteMTCTask" solution="{move_to_approach_solution}"/>
+        <Action ID="SaveCurrentState" saved_robot_state="{pre_approach_robot_state}"/>
         <Control ID="Parallel" success_count="1" failure_count="1">
-          <SubTree ID="Force Exceeds Threshold" />
-          <Action ID="ExecuteMTCTask" solution="{push_solution}" />
+          <SubTree ID="Force Exceeds Threshold"/>
+          <Action ID="ExecuteMTCTask" solution="{push_solution}"/>
         </Control>
       </Control>
-      <SubTree
-        ID="Retreat To Initial Pose"
-        pre_approach_robot_state="{pre_approach_robot_state}"
-        initial_robot_state="{initial_robot_state}"
-        _collapsed="true"
-      />
+      <SubTree ID="Retreat To Initial Pose" pre_approach_robot_state="{pre_approach_robot_state}" initial_robot_state="{initial_robot_state}" _collapsed="true"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Push Button ML">
       <MetadataFields>
-        <Metadata subcategory="Perception - Machine Learning" />
+        <Metadata subcategory="Perception - Machine Learning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/re_zero_force_torque_sensors.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/re_zero_force_torque_sensors.xml
index ba301570..8c9ea1f6 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/re_zero_force_torque_sensors.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/re_zero_force_torque_sensors.xml
@@ -1,26 +1,18 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Re-Zero Force-Torque Sensors">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Re-Zero Force-Torque Sensors"
-    _description="Zero the force or torque sensors to mitigate accumulated drift error"
-    _favorite="false"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Re-Zero Force-Torque Sensors" _description="Zero the force or torque sensors to mitigate accumulated drift error" _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Wrap in a ForceSuccess as Sim does not have the zero_ftsensor service -->
       <Decorator ID="ForceSuccess">
-        <Action
-          ID="CallTriggerService"
-          service_name="/io_and_status_controller/zero_ftsensor"
-        />
+        <Action ID="CallTriggerService" service_name="/io_and_status_controller/zero_ftsensor"/>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Re-Zero Force-Torque Sensors">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/reactivate_gripper.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/reactivate_gripper.xml
index 9bdad264..7f99ee28 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/reactivate_gripper.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/reactivate_gripper.xml
@@ -1,22 +1,19 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Reactivate Gripper">
   <!-- ////////// -->
   <BehaviorTree ID="Reactivate Gripper" _description="Reactivate the gripper">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="CallTriggerService"
-        service_name="/robotiq_activation_controller/reactivate_gripper"
-      />
+      <Action ID="CallTriggerService" service_name="/robotiq_activation_controller/reactivate_gripper"/>
     </Control>
   </BehaviorTree>
   <!-- ////////// -->
   <TreeNodesModel>
     <Action ID="CallTriggerService">
-      <inout_port name="service_name" />
+      <inout_port name="service_name"/>
     </Action>
     <SubTree ID="Reactivate Gripper">
       <MetadataFields>
-        <Metadata subcategory="Grasping" />
+        <Metadata subcategory="Grasping"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml
index d11825d4..00627f9b 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml
@@ -1,66 +1,23 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Retreat To Initial Pose">
-  <BehaviorTree
-    ID="Retreat To Initial Pose"
-    _description=""
-    _subtreeOnly="true"
-  >
+  <BehaviorTree ID="Retreat To Initial Pose" _description="" _subtreeOnly="true">
     <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="close_cabinet_door_retreat"
-        controller_names="/joint_trajectory_controller"
-        task="{retreat_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{retreat_task}" />
-      <Action
-        ID="SetupMTCUpdateGroupCollisionRule"
-        name="AllowGripperCollisionWithOctomap"
-        group_name="gripper"
-        object_name="&lt;octomap&gt;"
-        allow_collision="true"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCCartesianMoveToJointState"
-        joint_state="{pre_approach_robot_state}"
-        planning_group_name="manipulator"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCUpdateGroupCollisionRule"
-        name="ForbidGripperCollisionWithOctomap"
-        group_name="gripper"
-        object_name="&lt;octomap&gt;"
-        allow_collision="false"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCPlanToJointState"
-        joint_state="{initial_robot_state}"
-        planning_group_name="manipulator"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{return_to_initial_waypoint_solution}"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{return_to_initial_waypoint_solution}"
-      />
+      <Action ID="InitializeMTCTask" task_id="close_cabinet_door_retreat" controller_names="/joint_trajectory_controller" task="{retreat_task}"/>
+      <Action ID="SetupMTCCurrentState" task="{retreat_task}"/>
+      <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" group_name="gripper" object_name="&lt;octomap&gt;" allow_collision="true" task="{retreat_task}"/>
+      <Action ID="SetupMTCCartesianMoveToJointState" joint_state="{pre_approach_robot_state}" planning_group_name="manipulator" task="{retreat_task}"/>
+      <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" group_name="gripper" object_name="&lt;octomap&gt;" allow_collision="false" task="{retreat_task}"/>
+      <Action ID="SetupMTCPlanToJointState" joint_state="{initial_robot_state}" planning_group_name="manipulator" task="{retreat_task}"/>
+      <Action ID="PlanMTCTask" solution="{return_to_initial_waypoint_solution}" task="{retreat_task}"/>
+      <Action ID="ExecuteMTCTask" solution="{return_to_initial_waypoint_solution}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Retreat To Initial Pose">
-      <input_port
-        name="pre_approach_robot_state"
-        default="{pre_approach_robot_state}"
-      />
-      <input_port name="initial_robot_state" default="{initial_robot_state}" />
+      <input_port name="pre_approach_robot_state" default="{pre_approach_robot_state}"/>
+      <input_port name="initial_robot_state" default="{initial_robot_state}"/>
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/sam_pick_object.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/sam_pick_object.xml
index 92a3c05c..0b2df231 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/sam_pick_object.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/sam_pick_object.xml
@@ -1,147 +1,38 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="SAM Pick Object">
   <!--//////////-->
-  <BehaviorTree
-    ID="SAM Pick Object"
-    _description="Uses SAM to segment out an arbitrary object and pick it."
-  >
+  <BehaviorTree ID="SAM Pick Object" _description="Uses SAM to segment out an arbitrary object and pick it.">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action
-        ID="LoadObjectiveParameters"
-        parameters="{pick_parameters}"
-        config_file_name="sam_pick_object_config.yaml"
-      />
-      <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"
-        _collapsed="true"
-      />
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.0"
-      />
+      <Action ID="LoadObjectiveParameters" parameters="{pick_parameters}" config_file_name="sam_pick_object_config.yaml"/>
+      <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" _collapsed="true"/>
+      <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
       <!--Get the point cloud from the wrist camera-->
-      <Action
-        ID="GetCameraInfo"
-        topic_name="/wrist_mounted_camera/color/camera_info"
-        message_out="{camera_info}"
-        timeout_sec="5.000000"
-      />
-      <Action
-        ID="GetPointCloud"
-        topic_name="/wrist_mounted_camera/depth/color/points"
-        message_out="{point_cloud}"
-        timeout_sec="5.000000"
-      />
-      <Action
-        ID="SendPointCloudToUI"
-        point_cloud="{point_cloud}"
-        pcd_topic="/pcd_pointcloud_captures"
-      />
-      <Action
-        ID="GetImage"
-        topic_name="/wrist_mounted_camera/color/image_raw"
-        message_out="{image}"
-        timeout_sec="5.000000"
-      />
+      <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}" timeout_sec="5.000000"/>
+      <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" timeout_sec="5.000000"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
+      <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}" timeout_sec="5.000000"/>
       <!--Segment out 2D masks and then convert to 3D masks and visualize in UI-->
-      <Action
-        ID="GetPointsFromUser"
-        point_prompts="Click on the object you want to pick."
-        point_names="clicked_object"
-        pixel_coords="{pixel_coords}"
-        view_name="/wrist_mounted_camera/color/image_raw"
-      />
-      <Action
-        ID="GetMasks2DAction"
-        image="{image}"
-        action_name="get_masks_2d_superseg"
-        min_confidence="0.8"
-        max_nms_iou="0.8"
-        min_relative_area="0.0002"
-        max_relative_area="0.25"
-        timeout_sec="15.0"
-        masks2d="{masks2d}"
-        valid_points="{pixel_coords}"
-        valid_classes=""
-      />
-      <Action
-        ID="GetMasks3DFromMasks2D"
-        point_cloud="{point_cloud}"
-        masks2d="{masks2d}"
-        camera_info="{camera_info}"
-        masks3d="{masks3d}"
-      />
+      <Action ID="GetPointsFromUser" point_prompts="Click on the object you want to pick." point_names="clicked_object" pixel_coords="{pixel_coords}" view_name="/wrist_mounted_camera/color/image_raw"/>
+      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_superseg" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0.0002" max_relative_area="0.25" timeout_sec="15.0" masks2d="{masks2d}" valid_points="{pixel_coords}" valid_classes=""/>
+      <Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
       <Decorator ID="ForEach" 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}"
-            pcd_topic="/pcd_pointcloud_captures"
-          />
+          <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
+          <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" pcd_topic="/pcd_pointcloud_captures"/>
         </Control>
       </Decorator>
       <!--Convert 3D masks into graspable objects-->
-      <Action
-        ID="GetGraspableObjectsFromMasks3D"
-        graspable_objects="{objects}"
-        plane_inlier_threshold="0.020000"
-        base_frame="world"
-        minimum_face_area="0"
-        masks3d="{masks3d}"
-        face_separation_threshold="0.025000"
-        point_cloud="{point_cloud}"
-      />
+      <Action ID="GetGraspableObjectsFromMasks3D" graspable_objects="{objects}" plane_inlier_threshold="0.020000" base_frame="world" minimum_face_area="0" masks3d="{masks3d}" face_separation_threshold="0.025000" point_cloud="{point_cloud}"/>
       <Decorator ID="ForEach" vector_in="{objects}" out="{object}">
         <Control ID="Sequence">
-          <Action
-            ID="InitializeMTCTask"
-            task="{mtc_task}"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            task_id=""
-          />
-          <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
-          <Action
-            ID="SetupMTCApproachGrasp"
-            parameters="{pick_parameters}"
-            target_object="{object}"
-            monitored_stage="{monitored_stage}"
-            task="{mtc_task}"
-          />
-          <Action
-            ID="SetupMTCGenerateCuboidGrasps"
-            parameters="{pick_parameters}"
-            target_object="{object}"
-            monitored_stage="{monitored_stage}"
-            task="{mtc_task}"
-          />
-          <Action
-            ID="SetupMTCRetractFromGrasp"
-            parameters="{pick_parameters}"
-            target_object="{object}"
-            task="{mtc_task}"
-          />
-          <Action
-            ID="PlanMTCTask"
-            solution="{mtc_solution}"
-            task="{mtc_task}"
-          />
-          <SubTree
-            ID="Wait for Trajectory Approval if User Available"
-            _collapsed="false"
-            solution="{mtc_solution}"
-          />
-          <Action ID="ExecuteMTCTask" solution="{mtc_solution}" />
+          <Action ID="InitializeMTCTask" task="{mtc_task}" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task_id=""/>
+          <Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
+          <Action ID="SetupMTCApproachGrasp" parameters="{pick_parameters}" target_object="{object}" monitored_stage="{monitored_stage}" task="{mtc_task}"/>
+          <Action ID="SetupMTCGenerateCuboidGrasps" parameters="{pick_parameters}" target_object="{object}" monitored_stage="{monitored_stage}" task="{mtc_task}"/>
+          <Action ID="SetupMTCRetractFromGrasp" parameters="{pick_parameters}" target_object="{object}" task="{mtc_task}"/>
+          <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/>
+          <SubTree ID="Wait for Trajectory Approval if User Available" _collapsed="false" solution="{mtc_solution}"/>
+          <Action ID="ExecuteMTCTask" solution="{mtc_solution}"/>
         </Control>
       </Decorator>
     </Control>
@@ -149,8 +40,8 @@
   <TreeNodesModel>
     <SubTree ID="SAM Pick Object">
       <MetadataFields>
-        <Metadata subcategory="Perception - Machine Learning" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Perception - Machine Learning"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/sample_april_tag.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/sample_april_tag.xml
index f2540984..5a699278 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/sample_april_tag.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/sample_april_tag.xml
@@ -1,63 +1,27 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Sample April Tag">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Sample April Tag"
-    _description="Samples the pose of an AprilTag and returns the average measurement."
-    _favorite="false"
-  >
+  <BehaviorTree ID="Sample April Tag" _description="Samples the pose of an AprilTag and returns the average measurement." _favorite="false">
     <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="LoadObjectiveParameters" config_file_name="{apriltag_config}" parameters="{parameters}"/>
+        <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}"
-            />
-            <Action
-              ID="DetectAprilTags"
-              image="{image}"
-              camera_info="{camera_info}"
-              parameters="{parameters}"
-              detections="{detections}"
-            />
-            <Action
-              ID="GetDetectionPose"
-              detections="{detections}"
-              target_id="{tag_id}"
-              target_label=""
-              detection_pose="{detection_pose}"
-            />
+            <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
+            <Action ID="DetectAprilTags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/>
+            <Action ID="GetDetectionPose" detections="{detections}" target_id="{tag_id}" target_label="" detection_pose="{detection_pose}"/>
           </Control>
         </Decorator>
-        <Action
-          ID="AveragePoseStamped"
-          pose_sample="{detection_pose}"
-          avg_pose="{avg_pose}"
-          max_distance="{max_distance}"
-          max_rotation="{max_rotation}"
-          num_samples="{num_samples}"
-          run_continuously="false"
-        />
+        <Action ID="AveragePoseStamped" pose_sample="{detection_pose}" avg_pose="{avg_pose}" max_distance="{max_distance}" max_rotation="{max_rotation}" num_samples="{num_samples}" run_continuously="false"/>
       </Control>
     </Decorator>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Sample April Tag">
       <MetadataFields>
-        <Metadata subcategory="Perception - Machine Learning" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Perception - Machine Learning"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/take_snapshot.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/take_snapshot.xml
index 70043791..63b06e16 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/take_snapshot.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/take_snapshot.xml
@@ -1,35 +1,18 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <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"
-  >
+  <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="UpdatePlanningSceneService"
-        point_cloud="{point_cloud}"
-        point_cloud_service="/point_cloud_service"
-      />
-      <Action
-        ID="SendPointCloudToUI"
-        point_cloud="{point_cloud}"
-        pcd_topic="/pcd_pointcloud_captures"
-      />
+      <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="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
+      <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Take Snapshot">
       <MetadataFields>
-        <Metadata subcategory="Perception - 3D Point Cloud" />
+        <Metadata subcategory="Perception - 3D Point Cloud"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/teach_apriltag_grasp_offset.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/teach_apriltag_grasp_offset.xml
index 591340f5..90b2c36f 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/teach_apriltag_grasp_offset.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/teach_apriltag_grasp_offset.xml
@@ -1,70 +1,17 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Teach Grasp Offset From Apriltag">
-  <BehaviorTree
-    ID="Teach Grasp Offset From Apriltag"
-    _description="Saves a grasp offset to yaml file from Apriltag observation using an interactive marker"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Teach Grasp Offset From Apriltag" _description="Saves a grasp offset to yaml file from Apriltag observation using an interactive marker" _favorite="true">
     <Control ID="Sequence" name="root">
-      <Action
-        ID="GetPointsFromUser"
-        view_name="/wrist_mounted_camera/color/image_raw"
-        point_names="grasp_pose"
-        point_prompts="Click on the Apriltag"
-        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}"
-      />
-      <SubTree
-        ID="Sample April Tag"
-        num_samples="5"
-        tag_id="1"
-        apriltag_config="51mm_apriltag_detection_config.yaml"
-        max_distance="0.02"
-        max_rotation="0.2"
-        avg_pose="{tag_pose}"
-      />
-      <Action
-        ID="TransformPoseFrame"
-        input_pose="{tag_pose}"
-        target_frame_id="world"
-        output_pose="{tag_pose_world}"
-      />
-      <Decorator
-        ID="ForEach"
-        vector_in="{adjusted_poses}"
-        out="{user_grasp_pose}"
-      >
+      <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Click on the Apriltag" 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}"/>
+      <SubTree ID="Sample April Tag" num_samples="5" tag_id="1" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+      <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
+      <Decorator ID="ForEach" vector_in="{adjusted_poses}" out="{user_grasp_pose}">
         <Control ID="Sequence">
-          <Action
-            ID="CalculatePoseOffset"
-            source_pose="{tag_pose_world}"
-            destination_pose="{user_grasp_pose}"
-            source_to_destination_pose="{tag_to_user_grasp_pose}"
-          />
-          <Action
-            ID="SavePoseToYaml"
-            yaml_filename="apriltag1_grasp_offset"
-            namespace="GraspOffset"
-            message="{tag_to_user_grasp_pose}"
-          />
+          <Action ID="CalculatePoseOffset" source_pose="{tag_pose_world}" destination_pose="{user_grasp_pose}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
+          <Action ID="SavePoseToYaml" yaml_filename="apriltag1_grasp_offset" namespace="GraspOffset" message="{tag_to_user_grasp_pose}"/>
         </Control>
       </Decorator>
     </Control>
@@ -72,7 +19,7 @@
   <TreeNodesModel>
     <SubTree ID="Teach April Tag Grasp Offset">
       <MetadataFields>
-        <Metadata subcategory="Perception - Machine Learning" />
+        <Metadata subcategory="Perception - Machine Learning"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/update_admittance_controller.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/update_admittance_controller.xml
index 536ad49d..b83dc4a5 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/update_admittance_controller.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/update_admittance_controller.xml
@@ -1,26 +1,18 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Update Admittance Controller">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Update Admittance Controller"
-    _description="Updates the admittance controller configurations according to a specified yaml."
-    _favorite="false"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Update Admittance Controller" _description="Updates the admittance controller configurations according to a specified yaml." _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Wrap in a ForceSuccess until we have a mock admittance_controller loaded for sim. -->
       <Decorator ID="ForceSuccess">
-        <Action
-          ID="UpdateAdmittanceController"
-          config_file_name="open_door_admittance_controller_config.yaml"
-        />
+        <Action ID="UpdateAdmittanceController" config_file_name="open_door_admittance_controller_config.yaml"/>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Update Admittance Controller">
       <MetadataFields>
-        <Metadata subcategory="Perception - Scene Constraints" />
+        <Metadata subcategory="Perception - Scene Constraints"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/visual_servo_teach_reference.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/visual_servo_teach_reference.xml
index 86e6c0d4..956eb649 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/visual_servo_teach_reference.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/visual_servo_teach_reference.xml
@@ -1,59 +1,20 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Visual Servo Teach Reference">
-  <BehaviorTree
-    ID="Visual Servo Teach Reference"
-    _description="Teach the pose reference used in `Visual Servo To Reference'"
-    _favorite="true"
-  >
+  <BehaviorTree ID="Visual Servo Teach Reference" _description="Teach the pose reference used in `Visual Servo To Reference'" _favorite="true">
     <Control ID="Sequence" name="root">
-      <SubTree
-        ID="Sample April Tag"
-        _collapsed="true"
-        num_samples="5"
-        tag_id="2"
-        apriltag_config="51mm_apriltag_detection_config.yaml"
-        max_distance="0.02"
-        max_rotation="0.2"
-        avg_pose="{tag_pose}"
-      />
-      <Action
-        ID="CreateStampedPose"
-        reference_frame="grasp_link"
-        position_xyz="0.0;0.0;0.0"
-        orientation_xyzw="0.0;0.0;0.0;1.0"
-        stamped_pose="{grasp_link_pose}"
-      />
-      <Action
-        ID="TransformPoseFrame"
-        input_pose="{tag_pose}"
-        target_frame_id="world"
-        output_pose="{tag_pose_world}"
-      />
-      <Action
-        ID="TransformPoseFrame"
-        input_pose="{grasp_link_pose}"
-        target_frame_id="world"
-        output_pose="{grasp_link_pose_world}"
-      />
-      <Action
-        ID="CalculatePoseOffset"
-        source_pose="{tag_pose_world}"
-        destination_pose="{grasp_link_pose_world}"
-        source_to_destination_pose="{tag_to_user_grasp_pose}"
-      />
-      <Action
-        ID="SavePoseToYaml"
-        yaml_filename="visual_servo_reference"
-        namespace="ServoReference"
-        message="{tag_to_user_grasp_pose}"
-      />
+      <SubTree ID="Sample April Tag" _collapsed="true" num_samples="5" tag_id="2" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" position_xyz="0.0;0.0;0.0" orientation_xyzw="0.0;0.0;0.0;1.0" stamped_pose="{grasp_link_pose}"/>
+      <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
+      <Action ID="TransformPoseFrame" input_pose="{grasp_link_pose}" target_frame_id="world" output_pose="{grasp_link_pose_world}"/>
+      <Action ID="CalculatePoseOffset" source_pose="{tag_pose_world}" destination_pose="{grasp_link_pose_world}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
+      <Action ID="SavePoseToYaml" yaml_filename="visual_servo_reference" namespace="ServoReference" message="{tag_to_user_grasp_pose}"/>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Visual Servo Teach Reference">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
-        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Basic Examples"/>
+        <Metadata runnable="true"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/visual_servo_to_reference.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/visual_servo_to_reference.xml
index 2a13e91d..cd8c98a3 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/visual_servo_to_reference.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/visual_servo_to_reference.xml
@@ -1,105 +1,36 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Visual Servo To Reference">
   <!-- ////////// -->
-  <BehaviorTree
-    ID="Visual Servo To Reference"
-    _description="Move in closed loop to a pose relative to an AprilTag"
-    _favorite="true"
-    _hardcoded="false"
-  >
+  <BehaviorTree ID="Visual Servo To Reference" _description="Move in closed loop to a pose relative to an AprilTag" _favorite="true" _hardcoded="false">
     <Control ID="Sequence">
-      <Action
-        ID="LoadObjectiveParameters"
-        config_file_name="visual_servo_reference.yaml"
-        parameters="{servo_reference_parameters}"
-      />
-      <Action ID="ActivateControllers" controller_names="servo_controller" />
+      <Action ID="LoadObjectiveParameters" config_file_name="visual_servo_reference.yaml" parameters="{servo_reference_parameters}"/>
+      <Action ID="ActivateControllers" controller_names="servo_controller"/>
       <Control ID="Sequence">
-        <SubTree
-          ID="Sample April Tag"
-          _collapsed="true"
-          num_samples="1"
-          tag_id="2"
-          apriltag_config="51mm_apriltag_detection_config.yaml"
-          max_distance="0.02"
-          max_rotation="0.2"
-          avg_pose="{tag_pose}"
-        />
-        <Action
-          ID="TransformPoseFrame"
-          input_pose="{tag_pose}"
-          target_frame_id="base_link"
-          output_pose="{tag_pose_base}"
-        />
-        <Action
-          ID="TransformPoseFromYaml"
-          input_pose="{tag_pose_base}"
-          parameter_namespace="ServoReference"
-          pose_parameters="{servo_reference_parameters}"
-          output_pose="{reference_pose}"
-        />
+        <SubTree ID="Sample April Tag" _collapsed="true" num_samples="1" tag_id="2" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+        <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="base_link" output_pose="{tag_pose_base}"/>
+        <Action ID="TransformPoseFromYaml" input_pose="{tag_pose_base}" parameter_namespace="ServoReference" pose_parameters="{servo_reference_parameters}" output_pose="{reference_pose}"/>
       </Control>
       <Control ID="Parallel" success_count="1" failure_count="1">
         <Decorator ID="KeepRunningUntilFailure">
           <Control ID="Sequence">
-            <SubTree
-              ID="Sample April Tag"
-              _collapsed="true"
-              num_samples="1"
-              tag_id="2"
-              apriltag_config="51mm_apriltag_detection_config.yaml"
-              max_distance="0.02"
-              max_rotation="0.2"
-              avg_pose="{tag_pose}"
-            />
-            <Action
-              ID="TransformPoseFrame"
-              input_pose="{tag_pose}"
-              target_frame_id="base_link"
-              output_pose="{tag_pose_base}"
-            />
+            <SubTree ID="Sample April Tag" _collapsed="true" num_samples="1" tag_id="2" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+            <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="base_link" output_pose="{tag_pose_base}"/>
           </Control>
         </Decorator>
-        <Action
-          ID="AveragePoseStamped"
-          run_continuously="true"
-          num_samples="5"
-          max_distance="0.04"
-          max_rotation="0.4"
-          pose_sample="{tag_pose_base}"
-          avg_pose="{tag_pose_avg}"
-        />
+        <Action ID="AveragePoseStamped" run_continuously="true" num_samples="5" max_distance="0.04" max_rotation="0.4" pose_sample="{tag_pose_base}" avg_pose="{tag_pose_avg}"/>
         <Decorator ID="KeepRunningUntilFailure">
           <Control ID="Sequence">
-            <Action
-              ID="TransformPoseFromYaml"
-              input_pose="{tag_pose_avg}"
-              parameter_namespace="ServoReference"
-              pose_parameters="{servo_reference_parameters}"
-              output_pose="{reference_pose}"
-            />
+            <Action ID="TransformPoseFromYaml" input_pose="{tag_pose_avg}" parameter_namespace="ServoReference" pose_parameters="{servo_reference_parameters}" output_pose="{reference_pose}"/>
           </Control>
         </Decorator>
-        <Action
-          ID="ServoTowardsPose"
-          planning_group_name="manipulator"
-          target_pose="{reference_pose}"
-          translational_gain="1.0"
-          rotational_gain="1.0"
-          max_translational_vel="0.1"
-          max_rotational_vel="0.1"
-          publish_rate="20"
-          exit_threshold_translation="0.01"
-          exit_threshold_rotation="0.01"
-          exit_threshold_time="0.1"
-        />
+        <Action ID="ServoTowardsPose" planning_group_name="manipulator" target_pose="{reference_pose}" translational_gain="1.0" rotational_gain="1.0" max_translational_vel="0.1" max_rotational_vel="0.1" publish_rate="20" exit_threshold_translation="0.01" exit_threshold_rotation="0.01" exit_threshold_time="0.1"/>
       </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Visual Servo to Reference">
       <MetadataFields>
-        <Metadata subcategory="Motion - Execute" />
+        <Metadata subcategory="Motion - Execute"/>
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/package.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/package.xml
index f42193e0..bb7d5ba1 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/package.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/package.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version='1.0' encoding='utf-8'?>
 <package format="3">
   <name>picknik_ur_site_config</name>
   <version>6.5.0</version>