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 35596a12..31a2650f 100644
--- a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
+++ b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
@@ -1,19 +1,43 @@
-<?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">
     <Control ID="Sequence">
-      <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="Pick Cube" joint_group_name="{joint_group_name}"/>
-      <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="{joint_group_name}" planner_interface="{planner_interface}" task="{mtc_task}"/>
-      <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="Place Cube" joint_group_name="{joint_group_name}"/>
-      <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="{joint_group_name}" planner_interface="{planner_interface}" task="{mtc_task}"/>
+      <Action
+        ID="RetrieveWaypoint"
+        waypoint_joint_state="{target_joint_state}"
+        waypoint_name="Pick Cube"
+        joint_group_name="{joint_group_name}"
+      />
+      <Action
+        ID="SetupMTCMoveToJointState"
+        joint_state="{target_joint_state}"
+        name="SetupMTCMoveToJointState"
+        planning_group_name="{joint_group_name}"
+        planner_interface="{planner_interface}"
+        task="{mtc_task}"
+      />
+      <Action
+        ID="RetrieveWaypoint"
+        waypoint_joint_state="{target_joint_state}"
+        waypoint_name="Place Cube"
+        joint_group_name="{joint_group_name}"
+      />
+      <Action
+        ID="SetupMTCMoveToJointState"
+        joint_state="{target_joint_state}"
+        name="SetupMTCMoveToJointState"
+        planning_group_name="{joint_group_name}"
+        planner_interface="{planner_interface}"
+        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="planner_interface" default="moveit_default"/>
+      <input_port name="joint_group_name" default="manipulator" />
+      <inout_port name="mtc_task" default="{mtc_task}" />
+      <input_port name="planner_interface" default="moveit_default" />
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/pick_object.xml b/src/lab_sim/objectives/pick_object.xml
index ac46e08c..2aa4018c 100644
--- a/src/lab_sim/objectives/pick_object.xml
+++ b/src/lab_sim/objectives/pick_object.xml
@@ -1 +1,32 @@
-<root BTCPP_format="4" main_tree_to_execute="Pick object"><!--//////////--><BehaviorTree ID="Pick object" _description="" _favorite="false" _subtreeOnly="true"><Control ID="Sequence" name="root"><SubTree ID="Open Gripper"/><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.515" stamped_pose="{stamped_pose}"/><Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_vector}"/></Control><SubTree ID="Pick first object in vector" pose_vector="{pose_vector}" _collapsed="false"/></Control></BehaviorTree></root>
\ No newline at end of file
+<root BTCPP_format="4" main_tree_to_execute="Pick object">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Pick object"
+    _description=""
+    _favorite="false"
+    _subtreeOnly="true"
+  >
+    <Control ID="Sequence" name="root">
+      <SubTree ID="Open Gripper" />
+      <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.515"
+          stamped_pose="{stamped_pose}"
+        />
+        <Action
+          ID="AddPoseStampedToVector"
+          input="{stamped_pose}"
+          vector="{pose_vector}"
+        />
+      </Control>
+      <SubTree
+        ID="Pick first object in vector"
+        pose_vector="{pose_vector}"
+        _collapsed="false"
+      />
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/lab_sim/objectives/pick_up_cube.xml b/src/lab_sim/objectives/pick_up_cube.xml
index e23c4dab..336abbae 100644
--- a/src/lab_sim/objectives/pick_up_cube.xml
+++ b/src/lab_sim/objectives/pick_up_cube.xml
@@ -1,13 +1,13 @@
-<?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="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="Wrist camera snapshot" _collapsed="true"/>
-      <SubTree ID="Pick object" _collapsed="false"/>
+      <SubTree ID="Wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Pick object" _collapsed="false" />
     </Control>
   </BehaviorTree>
 </root>
diff --git a/src/lab_sim/objectives/plan_and_save_trajectory.xml b/src/lab_sim/objectives/plan_and_save_trajectory.xml
index b3782ba2..a3c28efd 100644
--- a/src/lab_sim/objectives/plan_and_save_trajectory.xml
+++ b/src/lab_sim/objectives/plan_and_save_trajectory.xml
@@ -1,19 +1,56 @@
-<?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="" _favorite="true" _subtreeOnly="false">
+  <BehaviorTree
+    ID="Plan and Save Trajectory"
+    _description=""
+    _favorite="true"
+    _subtreeOnly="false"
+  >
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Workspace Right" joint_group_name="manipulator" planner_interface="moveit_default" controller_names="/joint_trajectory_controller"/>
-      <SubTree ID="Create Pose Vector" _collapsed="false" target_poses="{target_poses}"/>
-      <SubTree ID="Add Poses to MTC Task" _collapsed="true" target_poses="{target_poses}" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" ik_frame="grasp_link" mtc_task="{mtc_task}"/>
-      <SubTree ID="Add Waypoints to MTC Task" _collapsed="false" mtc_task="{mtc_task}" waypoint_names="Pick Cube;Place Cube" joint_group_name="manipulator" planner_interface="moveit_default"/>
-      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/>
-      <Action ID="WaitForUserTrajectoryApproval"/>
-      <Action ID="ConvertMtcSolutionToJointTrajectory" joint_trajectory="{joint_trajectory_msg}"/>
-      <Action ID="SaveJointTrajectoryToYaml" yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"/>
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        waypoint_name="Workspace Right"
+        joint_group_name="manipulator"
+        planner_interface="moveit_default"
+        controller_names="/joint_trajectory_controller"
+      />
+      <SubTree
+        ID="Create Pose Vector"
+        _collapsed="false"
+        target_poses="{target_poses}"
+      />
+      <SubTree
+        ID="Add Poses to MTC Task"
+        _collapsed="true"
+        target_poses="{target_poses}"
+        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+        planning_group_name="manipulator"
+        ik_frame="grasp_link"
+        mtc_task="{mtc_task}"
+      />
+      <SubTree
+        ID="Add Waypoints to MTC Task"
+        _collapsed="false"
+        mtc_task="{mtc_task}"
+        waypoint_names="Pick Cube;Place Cube"
+        joint_group_name="manipulator"
+        planner_interface="moveit_default"
+      />
+      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
+      <Action ID="WaitForUserTrajectoryApproval" />
+      <Action
+        ID="ConvertMtcSolutionToJointTrajectory"
+        joint_trajectory="{joint_trajectory_msg}"
+      />
+      <Action
+        ID="SaveJointTrajectoryToYaml"
+        yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Plan and Save Trajectory"/>
+    <SubTree ID="Plan and Save Trajectory" />
   </TreeNodesModel>
 </root>