From 8e669844693389f65dc78d66d1523a2f6c89c05c Mon Sep 17 00:00:00 2001
From: Michael Wrock <mike.wrock@gmail.com>
Date: Tue, 12 Nov 2024 08:57:41 -0800
Subject: [PATCH] formatting

---
 .pre-commit-config.yaml                       |   1 +
 src/fanuc_sim/objectives/move_to_waypoint.xml |  70 +-
 src/fanuc_sim/objectives/teleoperate.xml      |  20 +-
 src/fanuc_sim/waypoints/waypoints.yaml        |   2 +-
 .../objectives/3_waypoint_pick_and_place.xml  | 123 ++-
 .../objectives/add_waypoints_to_mtc_task.xml  |  32 +-
 .../objectives/apriltag_pick_object.xml       |  72 +-
 .../objectives/constrained_pick_place.xml     | 152 ++-
 src/lab_sim/objectives/create_pose_vector.xml | 112 ++-
 .../objectives/cycle_between_waypoints.xml    |  29 +-
 src/lab_sim/objectives/joint_diagnostic.xml   |  39 +-
 src/lab_sim/objectives/joint_trajectory.yaml  |   2 +-
 .../load_and_execute_joint_trajectory.xml     |  24 +-
 src/lab_sim/objectives/look_at_table.xml      |  16 +-
 .../move_with_velocity_and_force.xml          |  76 +-
 .../objectives/pick_and_place_example.xml     |  75 +-
 src/lab_sim/objectives/pick_up_cube.xml       |  13 +-
 src/lab_sim/objectives/place_object.xml       |  30 +-
 .../objectives/plan_and_save_trajectory.xml   |  57 +-
 src/lab_sim/objectives/plan_to_pose.xml       |  52 +-
 src/lab_sim/objectives/record_trajectory.xml  |  30 +-
 src/lab_sim/objectives/register_cad_part.xml  |  56 +-
 src/lab_sim/objectives/take_snap.xml          |  15 +-
 src/lab_sim/objectives/visualize_pose.xml     |  10 +-
 src/lab_sim/objectives/wrist_snap.xml         |  14 +-
 .../launch/agent_bridge.launch.xml            |  10 +-
 .../objectives/3_waypoint_pick_and_place.xml  | 103 +-
 .../objectives/clear_snapshot.xml             |  15 +-
 .../objectives/close_gripper.xml              |  16 +-
 .../objectives/cycle_between_waypoints.xml    |  33 +-
 ...t_imarker_pose_from_mesh_visualization.xml |  32 +-
 .../objectives/interpolate_to_joint_state.xml |  74 +-
 .../objectives/move_along_path.xml            |  99 +-
 .../objectives/move_to_joint_state.xml        |  68 +-
 .../objectives/move_to_pose.xml               |  73 +-
 .../objectives/move_to_waypoint.xml           |  43 +-
 .../objectives/open_gripper.xml               |  18 +-
 .../objectives/reactivate_gripper.xml         |  31 +-
 .../objectives/request_teleoperation.xml      |  41 +-
 .../objectives/reset_plannning_scene.xml      |  14 +-
 .../objectives/teleoperate.xml                |  16 +-
 ..._trajectory_approval_if_user_available.xml |  16 +-
 .../kinova_gen3_base_config/package.xml       |   2 +-
 .../description/mujoco/gen3_7dof.xml          | 818 +++++++++++----
 .../mujoco/gen3_7dof_rafti_fingers.xml        | 938 ++++++++++++++----
 .../description/mujoco/landsat_scene.xml      | 193 ++--
 .../mujoco/rafti_fingers_scene.xml            | 186 ++--
 .../description/mujoco/scene.xml              | 186 ++--
 .../launch/agent_bridge.launch.xml            |   8 +-
 .../april_tag_object_registration.xml         | 260 ++++-
 .../objectives/clear_snapshot.xml             |  15 +-
 .../objectives/close_gripper.xml              |   8 +-
 .../objectives/compliant_grasp_landsat.xml    | 104 +-
 .../objectives/compliant_grasp_rafti.xml      | 257 ++++-
 .../objectives/compliant_grasp_rafti_vfc.xml  | 274 ++++-
 .../objectives/constrained_pick_place.xml     |  40 +-
 .../objectives/cycle_between_waypoints.xml    |  33 +-
 .../objectives/interpolate_to_joint_state.xml |  74 +-
 .../objectives/joint_diagnostic.xml           |  29 +-
 .../objectives/move_along_path.xml            | 108 +-
 .../objectives/move_along_path_admittance.xml | 119 ++-
 .../objectives/move_to_joint_state.xml        |  68 +-
 .../objectives/move_to_pose.xml               |  72 +-
 .../objectives/open_gripper.xml               |  18 +-
 .../re_zero_force_torque_sensors.xml          |  16 +-
 .../objectives/reactivate_gripper.xml         |  31 +-
 .../objectives/request_teleoperation.xml      |  41 +-
 .../objectives/reset_planning_scene.xml       |  14 +-
 .../objectives/sample_april_tag.xml           |  54 +-
 .../objectives/take_snapshot.xml              |  29 +-
 .../objectives/teleoperate.xml                |  16 +-
 .../objectives/test.xml                       |  15 +-
 .../velocity_force_controller_test.xml        |  36 +-
 .../velocity_force_controller_zero.xml        |  34 +-
 .../visual_servo_teach_reference.xml          |  55 +-
 .../objectives/visual_servo_to_reference.xml  |  93 +-
 ..._trajectory_approval_if_user_available.xml |  16 +-
 .../objectives/write_picknik.xml              |  63 +-
 .../kinova_gen3_mujoco_config/package.xml     |   6 +-
 .../package.xml                               |   7 +-
 src/moveit_pro_mobile_manipulation/README.md  |   2 +-
 src/moveit_pro_ur_configs/README.md           |   2 +-
 .../objectives/move_to_waypoint.xml           |  67 +-
 .../objectives/pick_up_cube.xml               |  22 +-
 .../objectives/record_trajectory.xml          |  41 +-
 .../objectives/dsfxghsedf.xml                 |  14 +-
 .../objectives/dzfgsedr.xml                   |  22 +-
 87 files changed, 4976 insertions(+), 1354 deletions(-)

diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 23e59a05..ba80eeea 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -11,6 +11,7 @@
 #     pre-commit autoupdate
 #
 # See https://github.com/pre-commit/pre-commit
+exclude: ^src/external_dependencies/
 repos:
   # Standard hooks
   - repo: https://github.com/pre-commit/pre-commit-hooks
diff --git a/src/fanuc_sim/objectives/move_to_waypoint.xml b/src/fanuc_sim/objectives/move_to_waypoint.xml
index 6e9ca2a0..ca42e19e 100644
--- a/src/fanuc_sim/objectives/move_to_waypoint.xml
+++ b/src/fanuc_sim/objectives/move_to_waypoint.xml
@@ -1,20 +1,52 @@
 <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">
-        <Control ID="Sequence" name="root">
-            <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}" joint_group_name="{joint_group_name}"/>
-            <Action ID="InitializeMTCTask" task_id="move_to_named_pose" controller_names="{controller_names}" task="{move_to_waypoint_task}"/>
-            <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
-            <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="{joint_group_name}" planner_interface="{planner_interface}" constraints="{constraints}" task="{move_to_waypoint_task}"/>
-            <Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
-            <Action ID="BlockingExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
-        </Control>
-    </BehaviorTree>
-    <TreeNodesModel>
-        <SubTree ID="Move to Waypoint">
-            <input_port name="waypoint_name" default="Point A"/>
-            <input_port name="joint_group_name" default="manipulator"/>
-            <input_port name="planner_interface" default="moveit_default"/>
-            <input_port name="controller_names" default="/joint_trajectory_controller"/>
-        </SubTree>
-    </TreeNodesModel>
-</root>
\ No newline at end of file
+  <BehaviorTree
+    ID="Move to Waypoint"
+    _description="This Objective is used when moving to one of the saved waypoints in your site configuration"
+    _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}"
+      />
+      <Action
+        ID="InitializeMTCTask"
+        task_id="move_to_named_pose"
+        controller_names="{controller_names}"
+        task="{move_to_waypoint_task}"
+      />
+      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
+      <Action
+        ID="SetupMTCMoveToJointState"
+        joint_state="{target_joint_state}"
+        name="SetupMTCMoveToJointState"
+        planning_group_name="{joint_group_name}"
+        planner_interface="{planner_interface}"
+        constraints="{constraints}"
+        task="{move_to_waypoint_task}"
+      />
+      <Action
+        ID="PlanMTCTask"
+        solution="{move_to_waypoint_solution}"
+        task="{move_to_waypoint_task}"
+      />
+      <Action
+        ID="BlockingExecuteMTCTask"
+        solution="{move_to_waypoint_solution}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Move to Waypoint">
+      <input_port name="waypoint_name" default="Point A" />
+      <input_port name="joint_group_name" default="manipulator" />
+      <input_port name="planner_interface" default="moveit_default" />
+      <input_port
+        name="controller_names"
+        default="/joint_trajectory_controller"
+      />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/fanuc_sim/objectives/teleoperate.xml b/src/fanuc_sim/objectives/teleoperate.xml
index 88f087eb..69fb6eca 100644
--- a/src/fanuc_sim/objectives/teleoperate.xml
+++ b/src/fanuc_sim/objectives/teleoperate.xml
@@ -1,6 +1,16 @@
 <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." _favorite="false" _hardcoded="false">
-        <SubTree ID="Request Teleoperation" enable_user_interaction="false" user_interaction_prompt="" initial_teleop_mode="3"/>
-    </BehaviorTree>
-</root>
\ No newline at end of file
+  <!--//////////-->
+  <BehaviorTree
+    ID="Teleoperate"
+    _description="Handles the different variations of teleoperation from the web UI. Can be used standalone."
+    _favorite="false"
+    _hardcoded="false"
+  >
+    <SubTree
+      ID="Request Teleoperation"
+      enable_user_interaction="false"
+      user_interaction_prompt=""
+      initial_teleop_mode="3"
+    />
+  </BehaviorTree>
+</root>
diff --git a/src/fanuc_sim/waypoints/waypoints.yaml b/src/fanuc_sim/waypoints/waypoints.yaml
index 0637a088..fe51488c 100644
--- a/src/fanuc_sim/waypoints/waypoints.yaml
+++ b/src/fanuc_sim/waypoints/waypoints.yaml
@@ -1 +1 @@
-[]
\ No newline at end of file
+[]
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 584a6148..9693c3f9 100644
--- a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
+++ b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
@@ -1 +1,122 @@
-<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="false"><Control ID="Sequence"><!--Reset the planning scene to ensure no old collision objects are hanging around--><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 Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/><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 /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/><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 /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/><SubTree ID="Move to Waypoint" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}" 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 /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/></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 /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/><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 /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/><SubTree ID="Move to Waypoint" waypoint_name="Pick Cube" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/><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 /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/></Control></Control></Decorator></Control></BehaviorTree></root>
\ No newline at end of file
+<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="false"
+  >
+    <Control ID="Sequence">
+      <!--Reset the planning scene to ensure no old collision objects are hanging around-->
+      <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 Table"
+        joint_group_name="manipulator"
+        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+        planner_interface="moveit_default"
+        constraints="{constraints}"
+      />
+      <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 /robotiq_gripper_controller"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+            />
+            <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 /robotiq_gripper_controller"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+            />
+            <SubTree
+              ID="Move to Waypoint"
+              joint_group_name="manipulator"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+              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 /robotiq_gripper_controller"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+            />
+          </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 /robotiq_gripper_controller"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+            />
+            <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 /robotiq_gripper_controller"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+            />
+            <SubTree
+              ID="Move to Waypoint"
+              waypoint_name="Pick Cube"
+              joint_group_name="manipulator"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+            />
+            <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 /robotiq_gripper_controller"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+            />
+          </Control>
+        </Control>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+</root>
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 2d465825..04947544 100644
--- a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
+++ b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
@@ -1,22 +1,38 @@
-<?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">
-      <Decorator ID="ForEachString" vector_in="{waypoint_names}" out="{waypoint_name}">
+      <Decorator
+        ID="ForEachString"
+        vector_in="{waypoint_names}"
+        out="{waypoint_name}"
+      >
         <Control ID="Sequence">
-          <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}" 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="{waypoint_name}"
+            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>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Add Waypoints to MTC Task">
-      <input_port name="joint_group_name" default="manipulator"/>
-      <input_port name="waypoint_names" default="Pick Cube;Place Cube"/>
-      <inout_port name="mtc_task" default="{mtc_task}"/>
-      <input_port name="planner_interface" default="moveit_default"/>
+      <input_port name="joint_group_name" default="manipulator" />
+      <input_port name="waypoint_names" default="Pick Cube;Place Cube" />
+      <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/apriltag_pick_object.xml b/src/lab_sim/objectives/apriltag_pick_object.xml
index 6f82ae0a..6fb0d97e 100644
--- a/src/lab_sim/objectives/apriltag_pick_object.xml
+++ b/src/lab_sim/objectives/apriltag_pick_object.xml
@@ -1 +1,71 @@
-<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="false"><Control ID="Sequence" name="TopLevelSequence"><SubTree ID="Look at table" _collapsed="true"/><Action ID="ClearSnapshot"/><SubTree ID="Wrist camera snapshot" _collapsed="true"/><Control ID="Sequence" name="Setup"><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="TransformPose" output_pose="{offset_pose}" input_pose="{output_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;-0.01"/><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="{offset_pose}" task="{pick_object_task}" parameters="{parameters}"/><Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/><Action ID="ExecuteMTCTask" solution="{pick_object_solution}"/></Control></BehaviorTree></root>
\ No newline at end of file
+<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="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree ID="Look at table" _collapsed="true" />
+      <Action ID="ClearSnapshot" />
+      <SubTree ID="Wrist camera snapshot" _collapsed="true" />
+      <Control ID="Sequence" name="Setup">
+        <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="TransformPose"
+        output_pose="{offset_pose}"
+        input_pose="{output_pose}"
+        quaternion_xyzw="0;0;0;1"
+        translation_xyz="0;0;-0.01"
+      />
+      <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="{offset_pose}"
+        task="{pick_object_task}"
+        parameters="{parameters}"
+      />
+      <Action
+        ID="PlanMTCTask"
+        solution="{pick_object_solution}"
+        task="{pick_object_task}"
+      />
+      <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" />
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/lab_sim/objectives/constrained_pick_place.xml b/src/lab_sim/objectives/constrained_pick_place.xml
index a85428fd..4885df26 100644
--- a/src/lab_sim/objectives/constrained_pick_place.xml
+++ b/src/lab_sim/objectives/constrained_pick_place.xml
@@ -1 +1,151 @@
-<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="false"><Control ID="Sequence"><SubTree ID="Move to Waypoint" _collapsed="true" constraints="{constraints}" waypoint_name="Look at Table" joint_group_name="manipulator" planner_interface="moveit_default" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/><SubTree ID="Open Gripper"/><Decorator ID="KeepRunningUntilFailure"><Control ID="Sequence"><!--Create a special type of motion planning configuration that includes an upwards orientation requirement--><Action ID="InitializeMotionConstraints" constraints_name="gripper pointing down" constraints="{constraints}"/><Action ID="AppendOrientationConstraint" config_file_name="my_constraints.yaml" constraints="{constraints}"/><!--Move to pick location--><!--Note: Sampling based planners can be non-deterministic. The retry decorator improves the likelihood of success--><Decorator ID="RetryUntilSuccessful" num_attempts="3"><SubTree ID="Move to Waypoint" waypoint_name="Above Pick Cube" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/></Decorator><Decorator ID="RetryUntilSuccessful" num_attempts="3"><SubTree ID="Move to Waypoint" waypoint_name="Pick Cube" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/></Decorator><!--Pick--><SubTree ID="Close Gripper"/><!--Move to place (drop) location--><Decorator ID="RetryUntilSuccessful" num_attempts="3"><SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" constraints="{constraints}" _collapsed="true" waypoint_name="Above Pick Cube"/></Decorator><Decorator ID="RetryUntilSuccessful" num_attempts="3"><SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" constraints="{constraints}" _collapsed="true" waypoint_name="Above Place Cube"/></Decorator><Decorator ID="RetryUntilSuccessful" num_attempts="3"><SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" constraints="{constraints}" _collapsed="true" waypoint_name="Place Cube"/></Decorator><SubTree ID="Open Gripper"/><Decorator ID="RetryUntilSuccessful" num_attempts="3"><SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" constraints="{constraints}" _collapsed="true" waypoint_name="Above Place Cube"/></Decorator><Decorator ID="RetryUntilSuccessful" num_attempts="3"><SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" constraints="{constraints}" _collapsed="true" waypoint_name="Place Cube"/></Decorator><SubTree ID="Close Gripper"/><Decorator ID="RetryUntilSuccessful" num_attempts="3"><SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" constraints="{constraints}" _collapsed="true" waypoint_name="Above Place Cube"/></Decorator><Decorator ID="RetryUntilSuccessful" num_attempts="3"><SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" constraints="{constraints}" _collapsed="true" waypoint_name="Above Pick Cube"/></Decorator><Decorator ID="RetryUntilSuccessful" num_attempts="3"><SubTree ID="Move to Waypoint" waypoint_name="Pick Cube" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/></Decorator><SubTree ID="Open Gripper"/></Control></Decorator></Control></BehaviorTree></root>
\ No newline at end of file
+<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="false"
+  >
+    <Control ID="Sequence">
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        constraints="{constraints}"
+        waypoint_name="Look at Table"
+        joint_group_name="manipulator"
+        planner_interface="moveit_default"
+        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+      />
+      <SubTree ID="Open Gripper" />
+      <Decorator ID="KeepRunningUntilFailure">
+        <Control ID="Sequence">
+          <!--Create a special type of motion planning configuration that includes an upwards orientation requirement-->
+          <Action
+            ID="InitializeMotionConstraints"
+            constraints_name="gripper pointing down"
+            constraints="{constraints}"
+          />
+          <Action
+            ID="AppendOrientationConstraint"
+            config_file_name="my_constraints.yaml"
+            constraints="{constraints}"
+          />
+          <!--Move to pick location-->
+          <!--Note: Sampling based planners can be non-deterministic. The retry decorator improves the likelihood of success-->
+          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+            <SubTree
+              ID="Move to Waypoint"
+              waypoint_name="Above Pick Cube"
+              joint_group_name="manipulator"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+            />
+          </Decorator>
+          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+            <SubTree
+              ID="Move to Waypoint"
+              waypoint_name="Pick Cube"
+              joint_group_name="manipulator"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+            />
+          </Decorator>
+          <!--Pick-->
+          <SubTree ID="Close Gripper" />
+          <!--Move to place (drop) location-->
+          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+            <SubTree
+              ID="Move to Waypoint"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+              joint_group_name="manipulator"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+              _collapsed="true"
+              waypoint_name="Above Pick Cube"
+            />
+          </Decorator>
+          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+            <SubTree
+              ID="Move to Waypoint"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+              joint_group_name="manipulator"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+              _collapsed="true"
+              waypoint_name="Above Place Cube"
+            />
+          </Decorator>
+          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+            <SubTree
+              ID="Move to Waypoint"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+              joint_group_name="manipulator"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+              _collapsed="true"
+              waypoint_name="Place Cube"
+            />
+          </Decorator>
+          <SubTree ID="Open Gripper" />
+          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+            <SubTree
+              ID="Move to Waypoint"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+              joint_group_name="manipulator"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+              _collapsed="true"
+              waypoint_name="Above Place Cube"
+            />
+          </Decorator>
+          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+            <SubTree
+              ID="Move to Waypoint"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+              joint_group_name="manipulator"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+              _collapsed="true"
+              waypoint_name="Place Cube"
+            />
+          </Decorator>
+          <SubTree ID="Close Gripper" />
+          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+            <SubTree
+              ID="Move to Waypoint"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+              joint_group_name="manipulator"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+              _collapsed="true"
+              waypoint_name="Above Place Cube"
+            />
+          </Decorator>
+          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+            <SubTree
+              ID="Move to Waypoint"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+              joint_group_name="manipulator"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+              _collapsed="true"
+              waypoint_name="Above Pick Cube"
+            />
+          </Decorator>
+          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+            <SubTree
+              ID="Move to Waypoint"
+              waypoint_name="Pick Cube"
+              joint_group_name="manipulator"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+              planner_interface="moveit_default"
+              constraints="{constraints}"
+            />
+          </Decorator>
+          <SubTree ID="Open Gripper" />
+        </Control>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/lab_sim/objectives/create_pose_vector.xml b/src/lab_sim/objectives/create_pose_vector.xml
index 9f8ff718..2b563292 100644
--- a/src/lab_sim/objectives/create_pose_vector.xml
+++ b/src/lab_sim/objectives/create_pose_vector.xml
@@ -1,31 +1,103 @@
-<?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">
     <Control ID="Sequence">
-      <Action ID="CreateStampedPose" orientation_xyzw="0;-1;0;0" position_xyz="1;0.5;1"/>
-      <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"/>
-      <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"/>
-      <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"/>
-      <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"/>
-      <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"/>
-      <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"/>
-      <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"/>
-      <Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
-      <Action ID="CreateStampedPose" orientation_xyzw="0;-1;0;0" position_xyz="-1;0.5;1"/>
-      <Action ID="AddPoseStampedToVector" vector="{target_poses}" input="{stamped_pose}"/>
+      <Action
+        ID="CreateStampedPose"
+        orientation_xyzw="0;-1;0;0"
+        position_xyz="1;0.5;1"
+      />
+      <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"
+      />
+      <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"
+      />
+      <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"
+      />
+      <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"
+      />
+      <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"
+      />
+      <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"
+      />
+      <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"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        vector="{target_poses}"
+        input="{stamped_pose}"
+      />
+      <Action
+        ID="CreateStampedPose"
+        orientation_xyzw="0;-1;0;0"
+        position_xyz="-1;0.5;1"
+      />
+      <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}" />
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/cycle_between_waypoints.xml b/src/lab_sim/objectives/cycle_between_waypoints.xml
index 1c022b36..71ff91b6 100644
--- a/src/lab_sim/objectives/cycle_between_waypoints.xml
+++ b/src/lab_sim/objectives/cycle_between_waypoints.xml
@@ -1 +1,28 @@
-<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"><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 /robotiq_gripper_controller" planner_interface="pro_rrt" constraints="{constraints}"/><SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="pro_rrt" constraints="{constraints}"/></Control></Decorator></BehaviorTree></root>
\ No newline at end of file
+<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"
+  >
+    <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 /robotiq_gripper_controller"
+          planner_interface="pro_rrt"
+          constraints="{constraints}"
+        />
+        <SubTree
+          ID="Move to Waypoint"
+          waypoint_name="Look at Table"
+          joint_group_name="manipulator"
+          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+          planner_interface="pro_rrt"
+          constraints="{constraints}"
+        />
+      </Control>
+    </Decorator>
+  </BehaviorTree>
+</root>
diff --git a/src/lab_sim/objectives/joint_diagnostic.xml b/src/lab_sim/objectives/joint_diagnostic.xml
index 7f2e71e2..1bbb7714 100644
--- a/src/lab_sim/objectives/joint_diagnostic.xml
+++ b/src/lab_sim/objectives/joint_diagnostic.xml
@@ -1 +1,38 @@
-<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="false"><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 /robotiq_gripper_controller" planner_interface="moveit_default"/><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" planner_interface="moveit_default"/><SubTree ID="Move to Waypoint" waypoint_name="Wrist 2 Min" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/></Control></Decorator></Control></BehaviorTree></root>
\ No newline at end of file
+<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="false"
+  >
+    <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 /robotiq_gripper_controller"
+        planner_interface="moveit_default"
+      />
+      <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"
+            planner_interface="moveit_default"
+          />
+          <SubTree
+            ID="Move to Waypoint"
+            waypoint_name="Wrist 2 Min"
+            joint_group_name="manipulator"
+            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+            planner_interface="moveit_default"
+          />
+        </Control>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/lab_sim/objectives/joint_trajectory.yaml b/src/lab_sim/objectives/joint_trajectory.yaml
index ed4bea93..defe73c1 100644
--- a/src/lab_sim/objectives/joint_trajectory.yaml
+++ b/src/lab_sim/objectives/joint_trajectory.yaml
@@ -40471,4 +40471,4 @@ points:
     effort: []
     time_from_start:
       sec: 14
-      nanosec: 440000000
\ No newline at end of file
+      nanosec: 440000000
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 f9e9e5c0..ba73574d 100644
--- a/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml
+++ b/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml
@@ -1,11 +1,25 @@
-<?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="" _favorite="true">
+  <BehaviorTree
+    ID="Load and Execute Joint Trajectory"
+    _description=""
+    _favorite="true"
+  >
     <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"/>
-      <Action ID="LoadJointTrajectoryFromYaml" file_path="/home/michael/user_ws/src/lab_sim/objectives/joint_trajectory.yaml"/>
-      <Action ID="ExecuteFollowJointTrajectory"/>
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        waypoint_name="Workspace Right"
+        joint_group_name="manipulator"
+        planner_interface="moveit_default"
+        controller_names="/joint_trajectory_controller"
+      />
+      <Action
+        ID="LoadJointTrajectoryFromYaml"
+        file_path="/home/michael/user_ws/src/lab_sim/objectives/joint_trajectory.yaml"
+      />
+      <Action ID="ExecuteFollowJointTrajectory" />
     </Control>
   </BehaviorTree>
 </root>
diff --git a/src/lab_sim/objectives/look_at_table.xml b/src/lab_sim/objectives/look_at_table.xml
index c07ce338..d7a11bed 100644
--- a/src/lab_sim/objectives/look_at_table.xml
+++ b/src/lab_sim/objectives/look_at_table.xml
@@ -1 +1,15 @@
-<root BTCPP_format="4" main_tree_to_execute="Look at table"><!--//////////--><BehaviorTree ID="Look at table" _description="" _favorite="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"/></Control></BehaviorTree></root>
\ No newline at end of file
+<root BTCPP_format="4" main_tree_to_execute="Look at table">
+  <!--//////////-->
+  <BehaviorTree ID="Look at table" _description="" _favorite="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"
+      />
+    </Control>
+  </BehaviorTree>
+</root>
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 aabc85f8..630d784b 100644
--- a/src/lab_sim/objectives/move_with_velocity_and_force.xml
+++ b/src/lab_sim/objectives/move_with_velocity_and_force.xml
@@ -1 +1,75 @@
-<root BTCPP_format="4" main_tree_to_execute="Move until Force Contact"><!--//////////--><BehaviorTree ID="Move until Force Contact" _description="An example for how to use the VFC to move with a velocity / force reference until it collides" _favorite="false"><Control ID="Sequence" name="TopLevelSequence"><SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="VFC Start" joint_group_name="manipulator" planner_interface="pro_rrt" 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"/></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"/></Control></BehaviorTree></root>
\ No newline at end of file
+<root BTCPP_format="4" main_tree_to_execute="Move until Force Contact">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Move until Force Contact"
+    _description="An example for how to use the VFC to move with a velocity / force reference until it collides"
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        waypoint_name="VFC Start"
+        joint_group_name="manipulator"
+        planner_interface="pro_rrt"
+        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"
+        />
+      </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"
+      />
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/lab_sim/objectives/pick_and_place_example.xml b/src/lab_sim/objectives/pick_and_place_example.xml
index f1d6253f..4529916f 100644
--- a/src/lab_sim/objectives/pick_and_place_example.xml
+++ b/src/lab_sim/objectives/pick_and_place_example.xml
@@ -1,25 +1,68 @@
-<?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="Pick and place example objective" _favorite="true" _hardcoded="false">
+  <BehaviorTree
+    ID="Pick And Place Example"
+    _description="Pick and place example objective"
+    _favorite="true"
+    _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}"/>
-        <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}"/>
+        <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}" />
+        <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}" />
       </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}"/>
-        <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="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}"/>
+        <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}" />
+        <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="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}" />
       </Control>
     </Control>
   </BehaviorTree>
-</root>
\ No newline at end of file
+</root>
diff --git a/src/lab_sim/objectives/pick_up_cube.xml b/src/lab_sim/objectives/pick_up_cube.xml
index c6d2506a..666ba3f1 100644
--- a/src/lab_sim/objectives/pick_up_cube.xml
+++ b/src/lab_sim/objectives/pick_up_cube.xml
@@ -1 +1,12 @@
-<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"/><!--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"/></Control></BehaviorTree></root>
\ No newline at end of file
+<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" />
+      <!--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" />
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/lab_sim/objectives/place_object.xml b/src/lab_sim/objectives/place_object.xml
index adc9e195..7f1ade17 100644
--- a/src/lab_sim/objectives/place_object.xml
+++ b/src/lab_sim/objectives/place_object.xml
@@ -1 +1,29 @@
-<root BTCPP_format="4" main_tree_to_execute="Place object"><!--//////////--><BehaviorTree ID="Place object" _description="" _favorite="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"/><Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{corrected_poses}"/></Control><!--Use the ForEach decorator to get a pose from our vector of poses--><SubTree ID="Place first object in vector" pose_vector="{corrected_poses}" _collapsed="false"/></Control></BehaviorTree><TreeNodesModel><SubTree ID="Place object"/></TreeNodesModel></root>
\ No newline at end of file
+<root BTCPP_format="4" main_tree_to_execute="Place object">
+  <!--//////////-->
+  <BehaviorTree ID="Place object" _description="" _favorite="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"
+        />
+        <Action
+          ID="AddPoseStampedToVector"
+          input="{stamped_pose}"
+          vector="{corrected_poses}"
+        />
+      </Control>
+      <!--Use the ForEach decorator to get a pose from our vector of poses-->
+      <SubTree
+        ID="Place first object in vector"
+        pose_vector="{corrected_poses}"
+        _collapsed="false"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Place object" />
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/plan_and_save_trajectory.xml b/src/lab_sim/objectives/plan_and_save_trajectory.xml
index 89ba336d..232e1f08 100644
--- a/src/lab_sim/objectives/plan_and_save_trajectory.xml
+++ b/src/lab_sim/objectives/plan_and_save_trajectory.xml
@@ -1,16 +1,53 @@
-<?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="true" 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="true"
+        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>
 </root>
diff --git a/src/lab_sim/objectives/plan_to_pose.xml b/src/lab_sim/objectives/plan_to_pose.xml
index 0ba21856..d52b7e56 100644
--- a/src/lab_sim/objectives/plan_to_pose.xml
+++ b/src/lab_sim/objectives/plan_to_pose.xml
@@ -1,27 +1,55 @@
-<?xml version="1.0" encoding="utf-8"?>
+<?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="" _favorite="false" _subtreeOnly="false">
+  <BehaviorTree
+    ID="Add Poses to MTC Task"
+    _description=""
+    _favorite="false"
+    _subtreeOnly="false"
+  >
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="InitializeMTCTask" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{mtc_task}"/>
-      <Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
+      <Action
+        ID="InitializeMTCTask"
+        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+        task="{mtc_task}"
+      />
+      <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
       <Action ID="Script" code="pose_ctr := 0" />
-      <Decorator ID="ForEachPoseStamped" vector_in="{target_poses}" out="{target_pose}" index="{index}">
+      <Decorator
+        ID="ForEachPoseStamped"
+        vector_in="{target_poses}"
+        out="{target_pose}"
+        index="{index}"
+      >
         <Control ID="Sequence">
-          <Action ID="VisualizePose" marker_name="{pose_ctr}" pose="{target_pose}"/>
+          <Action
+            ID="VisualizePose"
+            marker_name="{pose_ctr}"
+            pose="{target_pose}"
+          />
           <Action ID="Script" code="pose_ctr += 1" />
-          <Action ID="SetupMTCMoveToPose" ik_frame="{ik_frame}" planning_group_name="{planning_group_name}" target_pose="{target_pose}" task="{mtc_task}" planner_interface="moveit_default"/>
+          <Action
+            ID="SetupMTCMoveToPose"
+            ik_frame="{ik_frame}"
+            planning_group_name="{planning_group_name}"
+            target_pose="{target_pose}"
+            task="{mtc_task}"
+            planner_interface="moveit_default"
+          />
         </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}" />
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/record_trajectory.xml b/src/lab_sim/objectives/record_trajectory.xml
index d4a58ff9..e850d695 100644
--- a/src/lab_sim/objectives/record_trajectory.xml
+++ b/src/lab_sim/objectives/record_trajectory.xml
@@ -1 +1,29 @@
-<root BTCPP_format="4" main_tree_to_execute="Record Trajectory"><!--//////////--><BehaviorTree ID="Record Trajectory" _description="" _favorite="false"><Control ID="Sequence" name="TopLevelSequence"><Action ID="InitializeMTCTask" controller_names="/joint_trajectory_controller"/><Action ID="SetupMTCCurrentState"/><Decorator ID="ForEachString" vector_in="Point A;Point B;Point c" out="{waypoint_name}"><Control ID="Sequence" name="TopLevelSequence"><Action ID="LogMessage" message="{index}"/><Action ID="RetrieveWaypoint" joint_group_name="manipulator"/><Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" planner_interface="moveit_default"/></Control></Decorator><Action ID="PlanMTCTask"/><Action ID="WaitForUserTrajectoryApproval"/></Control></BehaviorTree></root>
\ No newline at end of file
+<root BTCPP_format="4" main_tree_to_execute="Record Trajectory">
+  <!--//////////-->
+  <BehaviorTree ID="Record Trajectory" _description="" _favorite="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="InitializeMTCTask"
+        controller_names="/joint_trajectory_controller"
+      />
+      <Action ID="SetupMTCCurrentState" />
+      <Decorator
+        ID="ForEachString"
+        vector_in="Point A;Point B;Point c"
+        out="{waypoint_name}"
+      >
+        <Control ID="Sequence" name="TopLevelSequence">
+          <Action ID="LogMessage" message="{index}" />
+          <Action ID="RetrieveWaypoint" joint_group_name="manipulator" />
+          <Action
+            ID="SetupMTCMoveToJointState"
+            joint_state="{target_joint_state}"
+            planner_interface="moveit_default"
+          />
+        </Control>
+      </Decorator>
+      <Action ID="PlanMTCTask" />
+      <Action ID="WaitForUserTrajectoryApproval" />
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/lab_sim/objectives/register_cad_part.xml b/src/lab_sim/objectives/register_cad_part.xml
index b9ffe6c0..83cdbb6e 100644
--- a/src/lab_sim/objectives/register_cad_part.xml
+++ b/src/lab_sim/objectives/register_cad_part.xml
@@ -1 +1,55 @@
-<root BTCPP_format="4" main_tree_to_execute="Register CAD Part"><!--//////////--><BehaviorTree ID="Register CAD Part" _description="" _favorite="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" name="Initial Guess"/><SubTree ID="Load Mesh as Red Pointcloud" _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 Pointcloud" _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></root>
\ No newline at end of file
+<root BTCPP_format="4" main_tree_to_execute="Register CAD Part">
+  <!--//////////-->
+  <BehaviorTree ID="Register CAD Part" _description="" _favorite="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"
+        name="Initial Guess"
+      />
+      <SubTree
+        ID="Load Mesh as Red Pointcloud"
+        _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 Pointcloud"
+        _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>
+</root>
diff --git a/src/lab_sim/objectives/take_snap.xml b/src/lab_sim/objectives/take_snap.xml
index cd79db33..a4aed6e5 100644
--- a/src/lab_sim/objectives/take_snap.xml
+++ b/src/lab_sim/objectives/take_snap.xml
@@ -1 +1,14 @@
-<root BTCPP_format="4" main_tree_to_execute="Scene camera snapshot"><!--//////////--><BehaviorTree ID="Scene camera snapshot" _description="" _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"/></Control></BehaviorTree><TreeNodesModel><SubTree ID="Scene camera snapshot"/></TreeNodesModel></root>
\ No newline at end of file
+<root BTCPP_format="4" main_tree_to_execute="Scene camera snapshot">
+  <!--//////////-->
+  <BehaviorTree ID="Scene camera snapshot" _description="" _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" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Scene camera snapshot" />
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/visualize_pose.xml b/src/lab_sim/objectives/visualize_pose.xml
index 410008c5..f9b1580f 100644
--- a/src/lab_sim/objectives/visualize_pose.xml
+++ b/src/lab_sim/objectives/visualize_pose.xml
@@ -1,10 +1,14 @@
-<?xml version="1.0" encoding="utf-8"?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Visualize Pose">
   <!--//////////-->
   <BehaviorTree ID="Visualize Pose" _description="" _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="CreateStampedPose" orientation_xyzw="0;0;-1;0" position_xyz="1;0.5;1"/>
-      <Action ID="VisualizePose"/>
+      <Action
+        ID="CreateStampedPose"
+        orientation_xyzw="0;0;-1;0"
+        position_xyz="1;0.5;1"
+      />
+      <Action ID="VisualizePose" />
     </Control>
   </BehaviorTree>
 </root>
diff --git a/src/lab_sim/objectives/wrist_snap.xml b/src/lab_sim/objectives/wrist_snap.xml
index 3749b530..be708d77 100644
--- a/src/lab_sim/objectives/wrist_snap.xml
+++ b/src/lab_sim/objectives/wrist_snap.xml
@@ -1 +1,13 @@
-<root BTCPP_format="4" main_tree_to_execute="Wrist camera snapshot"><!--//////////--><BehaviorTree ID="Wrist camera snapshot" _description="" _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"/></Control></BehaviorTree><TreeNodesModel><SubTree ID="Wrist camera snapshot"/></TreeNodesModel></root>
\ No newline at end of file
+<root BTCPP_format="4" main_tree_to_execute="Wrist camera snapshot">
+  <!--//////////-->
+  <BehaviorTree ID="Wrist camera snapshot" _description="" _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" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Wrist camera snapshot" />
+  </TreeNodesModel>
+</root>
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 1729a3c8..70ee0b38 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,5 +1,9 @@
-<?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_rest_api)/launch/rest_api.launch.xml"/>
+  <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_kinova_configs/kinova_gen3_base_config/objectives/3_waypoint_pick_and_place.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/3_waypoint_pick_and_place.xml
index 917be905..64786abb 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/3_waypoint_pick_and_place.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/3_waypoint_pick_and_place.xml
@@ -1,32 +1,101 @@
-<?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="Repeatedly grab a small object, place it at desired destination, and then navigate to specified waypoint until failure" _favorite="true">
+  <BehaviorTree
+    ID="3 Waypoints Pick and Place"
+    _description="Repeatedly grab a small object, place it at desired destination, and then navigate to specified waypoint until failure"
+    _favorite="true"
+  >
     <Control ID="Sequence">
       <!--Clear snapshot, move to center pose, and open gripper-->
-      <Action ID="ClearSnapshot"/>
-      <SubTree ID="Move to Waypoint" waypoint_name="Table" 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"/>
+      <Action ID="ClearSnapshot" />
+      <SubTree
+        ID="Move to Waypoint"
+        waypoint_name="Table"
+        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"
+      />
       <!--Keep executing the pick and place sequence until failure-->
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
           <!--Pick object from "Pick", put it down at "Place", and go back to center pose-->
           <Control ID="Sequence">
-            <SubTree ID="Move to Waypoint" waypoint_name="Pick" 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="Table" 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"/>
-            <SubTree ID="Move to Waypoint" waypoint_name="Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" />
+            <SubTree
+              ID="Move to Waypoint"
+              waypoint_name="Pick"
+              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="Table"
+              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"
+            />
+            <SubTree
+              ID="Move to Waypoint"
+              waypoint_name="Table"
+              joint_group_name="manipulator"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+            />
           </Control>
           <!--Pick object from "Place", put it down at "Pick", and go back to center 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.7929"/>
-            <SubTree ID="Move to Waypoint" waypoint_name="Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" />
-            <SubTree ID="Move to Waypoint" waypoint_name="Pick" 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="Table" 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.7929"
+            />
+            <SubTree
+              ID="Move to Waypoint"
+              waypoint_name="Table"
+              joint_group_name="manipulator"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+            />
+            <SubTree
+              ID="Move to Waypoint"
+              waypoint_name="Pick"
+              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="Table"
+              joint_group_name="manipulator"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+            />
           </Control>
         </Control>
       </Decorator>
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 2550ff7d..e8095851 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,7 +1,10 @@
-<?xml version="1.0"?>
-  <root BTCPP_format="4" main_tree_to_execute="Clear Snapshot">
-    <!-- ////////// -->
-    <BehaviorTree ID="Clear Snapshot" _description="Clear the point cloud snapshot">
-        <Action ID="ClearSnapshot" />
-    </BehaviorTree>
+<?xml version="1.0" ?>
+<root BTCPP_format="4" main_tree_to_execute="Clear Snapshot">
+  <!-- ////////// -->
+  <BehaviorTree
+    ID="Clear Snapshot"
+    _description="Clear the point cloud snapshot"
+  >
+    <Action ID="ClearSnapshot" />
+  </BehaviorTree>
 </root>
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 10ae314a..60d18f8c 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,8 +1,12 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <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"/>
-        </Control>
-    </BehaviorTree>
+  <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"
+      />
+    </Control>
+  </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/cycle_between_waypoints.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/cycle_between_waypoints.xml
index 4164da10..e9bf21b2 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/cycle_between_waypoints.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/cycle_between_waypoints.xml
@@ -1,11 +1,24 @@
-<?xml version="1.0"?>
-  <root BTCPP_format="4" main_tree_to_execute="Cycle Between Waypoints">
-    <BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure">
-        <Decorator ID="KeepRunningUntilFailure">
-            <Control ID="Sequence">
-                <SubTree ID="Move to Waypoint" waypoint_name="Pick" 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" />
-            </Control>
-        </Decorator>
-    </BehaviorTree>
+<?xml version="1.0" ?>
+<root BTCPP_format="4" main_tree_to_execute="Cycle Between Waypoints">
+  <BehaviorTree
+    ID="Cycle Between Waypoints"
+    _description="Cycles between two waypoints until failure"
+  >
+    <Decorator ID="KeepRunningUntilFailure">
+      <Control ID="Sequence">
+        <SubTree
+          ID="Move to Waypoint"
+          waypoint_name="Pick"
+          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"
+        />
+      </Control>
+    </Decorator>
+  </BehaviorTree>
 </root>
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 06d40f3d..9be4f855 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,19 +1,31 @@
-<?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" />
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/interpolate_to_joint_state.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/interpolate_to_joint_state.xml
index 3adb3dc7..ef1e22e5 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/interpolate_to_joint_state.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/interpolate_to_joint_state.xml
@@ -1,20 +1,58 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <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">
-        <Control ID="Fallback" name="root">
-            <Control ID="Sequence">
-                <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
-                <Action ID="InitializeMTCTask" task_id="interpolate_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_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="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"/>
-            </Control>
-        </Control>
-    </BehaviorTree>
+  <BehaviorTree
+    ID="Interpolate to Joint State"
+    _description="Move to a specified joint state using joint interpolation"
+  >
+    <Control ID="Fallback" name="root">
+      <Control ID="Sequence">
+        <Action
+          ID="RetrieveJointStateParameter"
+          timeout_sec="-1"
+          joint_state="{target_joint_state}"
+        />
+        <Action
+          ID="InitializeMTCTask"
+          task_id="interpolate_to_joint_state"
+          controller_names="/joint_trajectory_controller /robotiq_gripper_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="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" />
+      </Control>
+    </Control>
+  </BehaviorTree>
 </root>
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 4444e05e..802809cd 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,24 +1,95 @@
-<?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="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_pose="{stamped_pose}" pose_stamped_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_pose="{stamped_pose}" pose_stamped_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_pose="{stamped_pose}" pose_stamped_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_pose="{stamped_pose}" pose_stamped_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_pose="{stamped_pose}"
+          pose_stamped_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_pose="{stamped_pose}"
+          pose_stamped_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_pose="{stamped_pose}"
+          pose_stamped_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_pose="{stamped_pose}"
+          pose_stamped_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" 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}"/>
-          <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="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" joint_trajectory_msg="{joint_trajectory_msg}"/>
+          <Action
+            ID="PlanCartesianPath"
+            path="{pose_stamped_vector}"
+            planning_group_name="manipulator"
+            tip_link="end_effector_link"
+            tip_offset="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}"
+          />
+          <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="ExecuteFollowJointTrajectory"
+            execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
+            joint_trajectory_msg="{joint_trajectory_msg}"
+          />
         </Control>
       </Decorator>
     </Control>
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 9a352adf..a8270920 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,20 +1,52 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <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">
-        <Control ID="Fallback" name="root">
-            <Control ID="Sequence">
-                <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
-                <Action ID="InitializeMTCTask" task_id="move_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_waypoint_task}"/>
-                <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
-                <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}"  />
-                <Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
-                <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_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"/>
-            </Control>
-        </Control>
-    </BehaviorTree>
+  <BehaviorTree
+    ID="Move to Joint State"
+    _description="Move to a specified joint state"
+  >
+    <Control ID="Fallback" name="root">
+      <Control ID="Sequence">
+        <Action
+          ID="RetrieveJointStateParameter"
+          timeout_sec="-1"
+          joint_state="{target_joint_state}"
+        />
+        <Action
+          ID="InitializeMTCTask"
+          task_id="move_to_joint_state"
+          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+          task="{move_to_waypoint_task}"
+        />
+        <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
+        <Action
+          ID="SetupMTCMoveToJointState"
+          joint_state="{target_joint_state}"
+          name="SetupMTCMoveToJointState_First"
+          planning_group_name="manipulator"
+          task="{move_to_waypoint_task}"
+        />
+        <Action
+          ID="PlanMTCTask"
+          solution="{move_to_waypoint_solution}"
+          task="{move_to_waypoint_task}"
+        />
+        <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_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" />
+      </Control>
+    </Control>
+  </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_pose.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_pose.xml
index a52c754a..24d83b61 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_pose.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_pose.xml
@@ -1,21 +1,56 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <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">
-        <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="SetupMTCMoveToPose" 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"/>
-            </Control>
-        </Control>
-    </BehaviorTree>
+  <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="SetupMTCMoveToPose"
+          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" />
+      </Control>
+    </Control>
+  </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml
index eb299026..dd1dac8a 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml
@@ -1,18 +1,43 @@
-<?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">
+  <BehaviorTree
+    ID="Move to Waypoint"
+    _description="This Objective is used when moving to one of the saved waypoints in your site configuration"
+  >
     <Control ID="Sequence" name="root">
-      <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}" joint_group_name="{joint_group_name}"/>
-      <Action ID="InitializeMTCTask" task_id="move_to_named_pose" controller_names="{controller_names}" task="{move_to_waypoint_task}"/>
-      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
-      <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="{joint_group_name}" planner_interface="{planner_interface}" constraints="{constraints}" task="{move_to_waypoint_task}"/>
-      <Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
-      <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
+      <Action
+        ID="RetrieveWaypoint"
+        waypoint_joint_state="{target_joint_state}"
+        waypoint_name="{waypoint_name}"
+        joint_group_name="{joint_group_name}"
+      />
+      <Action
+        ID="InitializeMTCTask"
+        task_id="move_to_named_pose"
+        controller_names="{controller_names}"
+        task="{move_to_waypoint_task}"
+      />
+      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
+      <Action
+        ID="SetupMTCMoveToJointState"
+        joint_state="{target_joint_state}"
+        name="SetupMTCMoveToJointState"
+        planning_group_name="{joint_group_name}"
+        planner_interface="{planner_interface}"
+        constraints="{constraints}"
+        task="{move_to_waypoint_task}"
+      />
+      <Action
+        ID="PlanMTCTask"
+        solution="{move_to_waypoint_solution}"
+        task="{move_to_waypoint_task}"
+      />
+      <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Waypoint">
-      <input_port name="planner_interface" default="moveit_default"/>
+      <input_port name="planner_interface" default="moveit_default" />
     </SubTree>
   </TreeNodesModel>
 </root>
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 a05b6179..edf64720 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,9 +1,13 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <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"/>
-        </Control>
-    </BehaviorTree>
+  <!-- ////////// -->
+  <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"
+      />
+    </Control>
+  </BehaviorTree>
 </root>
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 a9666a46..3ba73b6a 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,16 +1,19 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <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" />
-        </Control>
-    </BehaviorTree>
-    <!-- ////////// -->
-    <TreeNodesModel>
-        <Action ID="CallTriggerService">
-            <inout_port name="service_name" />
-        </Action>
-    </TreeNodesModel>
-    <!-- ////////// -->
+  <!-- ////////// -->
+  <BehaviorTree ID="Reactivate Gripper" _description="Reactivate the gripper">
+    <Control ID="Sequence" name="root">
+      <Action
+        ID="CallTriggerService"
+        service_name="/robotiq_activation_controller/reactivate_gripper"
+      />
+    </Control>
+  </BehaviorTree>
+  <!-- ////////// -->
+  <TreeNodesModel>
+    <Action ID="CallTriggerService">
+      <inout_port name="service_name" />
+    </Action>
+  </TreeNodesModel>
+  <!-- ////////// -->
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/request_teleoperation.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/request_teleoperation.xml
index bf3d5b7e..90696f74 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/request_teleoperation.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/request_teleoperation.xml
@@ -1,35 +1,54 @@
-<?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 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">
-              <SubTree ID="Interpolate to Joint State"/>
+              <SubTree ID="Interpolate to Joint State" />
             </Decorator>
             <!-- Interactive markers move to pose -->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 4">
-              <SubTree ID="Move to Pose"/>
+              <SubTree ID="Move to Pose" />
             </Decorator>
             <!-- Waypoint buttons move to joint state -->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 3">
-              <SubTree ID="Move to Joint State"/>
+              <SubTree ID="Move to Joint State" />
             </Decorator>
             <!-- Cartesian and joint jogging -->
-            <Action ID="TeleoperateTwist" controller_name="servo_controller" _while="teleop_mode == 2"/>
-            <Action ID="TeleoperateJointJog" controller_name="servo_controller" _while="teleop_mode == 1"/>
+            <Action
+              ID="TeleoperateTwist"
+              controller_name="servo_controller"
+              _while="teleop_mode == 2"
+            />
+            <Action
+              ID="TeleoperateJointJog"
+              controller_name="servo_controller"
+              _while="teleop_mode == 1"
+            />
           </Control>
         </Decorator>
       </Control>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/reset_plannning_scene.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/reset_plannning_scene.xml
index 8e43ac5a..999da975 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/reset_plannning_scene.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/reset_plannning_scene.xml
@@ -1,7 +1,15 @@
-<?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="" _favorite="false" _hardcoded="false">
-    <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
+  <BehaviorTree
+    ID="Reset Planning Scene"
+    _description=""
+    _favorite="false"
+    _hardcoded="false"
+  >
+    <Action
+      ID="ResetPlanningSceneObjects"
+      apply_planning_scene_service="/apply_planning_scene"
+    />
   </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/teleoperate.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/teleoperate.xml
index f9b45da4..d230d7ee 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/teleoperate.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/teleoperate.xml
@@ -1,7 +1,17 @@
-<?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." _favorite="true" _hardcoded="false">
-    <SubTree ID="Request Teleoperation" enable_user_interaction="false" user_interaction_prompt="" initial_teleop_mode="3"/>
+  <BehaviorTree
+    ID="Teleoperate"
+    _description="Handles the different variations of teleoperation from the web UI. Can be used standalone."
+    _favorite="true"
+    _hardcoded="false"
+  >
+    <SubTree
+      ID="Request Teleoperation"
+      enable_user_interaction="false"
+      user_interaction_prompt=""
+      initial_teleop_mode="3"
+    />
   </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/wait_for_trajectory_approval_if_user_available.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/wait_for_trajectory_approval_if_user_available.xml
index 7abb4108..2f64f16a 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/wait_for_trajectory_approval_if_user_available.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/wait_for_trajectory_approval_if_user_available.xml
@@ -1,11 +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>
 </root>
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 bbde377c..ea20cb24 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" ?>
 <package format="3">
   <name>kinova_gen3_base_config</name>
   <version>5.1.0</version>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof.xml
index 0ce558f8..52672908 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof.xml
@@ -1,96 +1,179 @@
 <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>
@@ -98,133 +181,444 @@
   </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>
@@ -242,25 +636,43 @@
   </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>
-  
+
   <!--
     This adds stability to the model by having a tendon that distributes the forces between both
     joints, such that the equality constraint doesn't have to do that much work in order to equalize
@@ -269,25 +681,49 @@
   -->
   <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 name="ee_force_sensor" site="fts"/>
-   <torque name="ee_torque_sensor" site="fts" />
- </sensor>
-
+  <sensor>
+    <force name="ee_force_sensor" site="fts" />
+    <torque name="ee_torque_sensor" site="fts" />
+  </sensor>
 </mujoco>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof_rafti_fingers.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof_rafti_fingers.xml
index f010402e..2f220b14 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof_rafti_fingers.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof_rafti_fingers.xml
@@ -1,96 +1,179 @@
 <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>
@@ -98,152 +181,515 @@
   </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>
-<!--                              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>
                             </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>
-<!--                              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>
                             </body>
                           </body>
                         </body>
@@ -260,32 +706,68 @@
   </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"/>
-<!--                              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="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"
+    />
   </contact>
-  
+
   <!--
     This adds stability to the model by having a tendon that distributes the forces between both
     joints, such that the equality constraint doesn't have to do that much work in order to equalize
@@ -294,25 +776,49 @@
   -->
   <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 name="ee_force_sensor" site="fts"/>
-   <torque name="ee_torque_sensor" site="fts" />
- </sensor>
-
+  <sensor>
+    <force name="ee_force_sensor" site="fts" />
+    <torque name="ee_torque_sensor" site="fts" />
+  </sensor>
 </mujoco>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/landsat_scene.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/landsat_scene.xml
index 45ede67d..987fcaf4 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/landsat_scene.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/landsat_scene.xml
@@ -1,75 +1,134 @@
 <mujoco model="kinova scene">
-    <include file="gen3_7dof.xml"/>
-    <option gravity="0 0 0"/>
-    <option>
-        <flag multiccd="enable"/>
-    </option>
-    <option integrator="implicit"/>
-    <statistic center="0.3 0 0.4" extent="1"/>
-    <default>
-        <geom solref=".004 1"/>
-    </default>
+  <include file="gen3_7dof.xml" />
+  <option gravity="0 0 0" />
+  <option>
+    <flag multiccd="enable" />
+  </option>
+  <option integrator="implicit" />
+  <statistic center="0.3 0 0.4" extent="1" />
+  <default>
+    <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"/>
-    </visual>
+  <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" />
+  </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"/>
-    </asset>
+  <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" />
+  </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"/>
+  <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" />
+  </asset>
 
-    </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"
+    />
+  </worldbody>
 
-    <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"/>
-    </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"/>
-            <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"/>
-                <body name="april_tag" pos="0 -.1 .132" quat="1 0 0 0">
-                    <geom class="visual" mesh="april_tag" material="april_tag"/>
-                </body>
-            </body>
+  <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"
+      />
+      <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" />
+        <body name="april_tag" pos="0 -.1 .132" quat="1 0 0 0">
+          <geom class="visual" mesh="april_tag" material="april_tag" />
         </body>
-
-    </worldbody>
+      </body>
+    </body>
+  </worldbody>
 </mujoco>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/rafti_fingers_scene.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/rafti_fingers_scene.xml
index 946ea60e..80678631 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/rafti_fingers_scene.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/rafti_fingers_scene.xml
@@ -1,88 +1,139 @@
 <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"/>
+      <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" />
@@ -117,5 +168,4 @@
       <geom class="collision" mesh="ring_collision_32" />
     </body>
   </worldbody>
-
 </mujoco>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/scene.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/scene.xml
index df3736b6..1134c64b 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/scene.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/mujoco/scene.xml
@@ -1,88 +1,139 @@
 <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"/>
+      <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" />
@@ -117,5 +168,4 @@
       <geom class="collision" mesh="ring_collision_32" />
     </body>
   </worldbody>
-
 </mujoco>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/launch/agent_bridge.launch.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/launch/agent_bridge.launch.xml
index 40edf98a..4ca979bd 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/launch/agent_bridge.launch.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/launch/agent_bridge.launch.xml
@@ -1,4 +1,8 @@
 <launch>
-    <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/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_kinova_configs/kinova_gen3_mujoco_config/objectives/april_tag_object_registration.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/april_tag_object_registration.xml
index 53b7be3c..89beb80e 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/april_tag_object_registration.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/april_tag_object_registration.xml
@@ -1,83 +1,257 @@
-<?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="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/mock_satellite.stl" 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="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/mock_satellite.stl"
+            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>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/clear_snapshot.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/clear_snapshot.xml
index 2550ff7d..e8095851 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/clear_snapshot.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/clear_snapshot.xml
@@ -1,7 +1,10 @@
-<?xml version="1.0"?>
-  <root BTCPP_format="4" main_tree_to_execute="Clear Snapshot">
-    <!-- ////////// -->
-    <BehaviorTree ID="Clear Snapshot" _description="Clear the point cloud snapshot">
-        <Action ID="ClearSnapshot" />
-    </BehaviorTree>
+<?xml version="1.0" ?>
+<root BTCPP_format="4" main_tree_to_execute="Clear Snapshot">
+  <!-- ////////// -->
+  <BehaviorTree
+    ID="Clear Snapshot"
+    _description="Clear the point cloud snapshot"
+  >
+    <Action ID="ClearSnapshot" />
+  </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/close_gripper.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/close_gripper.xml
index a8807a3d..7d0fa975 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/close_gripper.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/close_gripper.xml
@@ -1,8 +1,12 @@
-<?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>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_landsat.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_landsat.xml
index b07be581..ce7e2526 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_landsat.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_landsat.xml
@@ -1,37 +1,95 @@
-<?xml version='1.0' encoding='UTF-8'?>
+<?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="Servo Start" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" />
-      <SubTree ID="Take Snapshot" _collapsed="false"/>
-      <SubTree ID="Sample April Tag" _collapsed="false" num_samples="1" apriltag_config="51mm_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="Servo Start"
+        joint_group_name="manipulator"
+        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+      />
+      <SubTree ID="Take Snapshot" _collapsed="false" />
+      <SubTree
+        ID="Sample April Tag"
+        _collapsed="false"
+        num_samples="1"
+        apriltag_config="51mm_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"/>
+          <Action
+            ID="PublishStaticFrame"
+            pose="{tag_pose}"
+            child_frame_id="tag_pose"
+          />
         </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.0995;0.2" reference_frame="tag_pose"/>
-      <Action ID="AddPoseStampedToVector" input="{rafti_pose}"/>
-      <Action ID="CreateStampedPose" stamped_pose="{grasp_pose}" orientation_xyzw="-0.707107;-0.707107;0;0" position_xyz="0;0.0995;0.0" reference_frame="tag_pose"/>
-      <Action ID="AddPoseStampedToVector" input="{grasp_pose}"/>
-      <Action ID="TransformPoseFrame" input_pose="{rafti_pose}" output_pose="{rafti_pose_world}"/>
+      <Action
+        ID="CreateStampedPose"
+        stamped_pose="{rafti_pose}"
+        orientation_xyzw="-0.707107;-0.707107;0;0"
+        position_xyz="0;0.0995;0.2"
+        reference_frame="tag_pose"
+      />
+      <Action ID="AddPoseStampedToVector" input="{rafti_pose}" />
+      <Action
+        ID="CreateStampedPose"
+        stamped_pose="{grasp_pose}"
+        orientation_xyzw="-0.707107;-0.707107;0;0"
+        position_xyz="0;0.0995;0.0"
+        reference_frame="tag_pose"
+      />
+      <Action ID="AddPoseStampedToVector" input="{grasp_pose}" />
+      <Action
+        ID="TransformPoseFrame"
+        input_pose="{rafti_pose}"
+        output_pose="{rafti_pose_world}"
+      />
       <Control ID="Fallback">
         <Decorator ID="Timeout" msec="100">
-          <Action ID="PublishStaticFrame" pose="{rafti_pose_world}" child_frame_id="rafti_pose"/>
+          <Action
+            ID="PublishStaticFrame"
+            pose="{rafti_pose_world}"
+            child_frame_id="rafti_pose"
+          />
         </Decorator>
-        <Action ID="AlwaysSuccess"/>
+        <Action ID="AlwaysSuccess" />
       </Control>
-      <Action ID="PlanCartesianPath" blending_radius="0.020000" position_only="false"/>
-      <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"/>
-      <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="Close Gripper" _collapsed="true"/>
+      <Action
+        ID="PlanCartesianPath"
+        blending_radius="0.020000"
+        position_only="false"
+      />
+      <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"
+      />
+      <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="Close Gripper" _collapsed="true" />
     </Control>
   </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml
index e2ca7db3..f795d79a 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml
@@ -1,79 +1,244 @@
-<?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 /robotiq_gripper_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 /robotiq_gripper_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="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/mock_satellite.stl" 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="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/mock_satellite.stl"
+            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="SetupMTCMoveToPose" 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="SetupMTCMoveToPose" 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>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml
index 15a7a3c3..758573d9 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml
@@ -1,84 +1,260 @@
-<?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 /robotiq_gripper_controller" use_all_planners="false"/>
-      <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 /robotiq_gripper_controller"
+        use_all_planners="false"
+      />
+      <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="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/mock_satellite.stl" 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="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/mock_satellite.stl"
+            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="SetupMTCMoveToPose" 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="SetupMTCMoveToPose" 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>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/constrained_pick_place.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/constrained_pick_place.xml
index 9b0b8420..063de3b0 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/constrained_pick_place.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/constrained_pick_place.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="Constrained Pick and Place">
   <!-- ////////// -->
-  <BehaviorTree ID="Constrained Pick and Place" _description="Pick and place an object with constraints on the gripper's range of motion" _favorite="true">
+  <BehaviorTree
+    ID="Constrained Pick and Place"
+    _description="Pick and place an object with constraints on the gripper's range of motion"
+    _favorite="true"
+  >
     <Decorator ID="KeepRunningUntilFailure">
       <Control ID="Sequence">
-        <Action ID="InitializeMotionConstraints" constraints_name="gripper pointing down" constraints="{constraints}"/>
-        <Action ID="AppendOrientationConstraint" config_file_name="my_constraints.yaml" constraints="{constraints}"/>
+        <Action
+          ID="InitializeMotionConstraints"
+          constraints_name="gripper pointing down"
+          constraints="{constraints}"
+        />
+        <Action
+          ID="AppendOrientationConstraint"
+          config_file_name="my_constraints.yaml"
+          constraints="{constraints}"
+        />
         <Control ID="Sequence" name="TopLevelSequence">
-          <SubTree ID="Open Gripper"/>
+          <SubTree ID="Open Gripper" />
           <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <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 Right"
+              joint_group_name="manipulator"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+            />
           </Decorator>
-          <SubTree ID="Close Gripper"/>
+          <SubTree ID="Close Gripper" />
           <Decorator ID="RetryUntilSuccessful" num_attempts="3">
-            <SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" waypoint_name="Grasp Left" planner_interface="moveit_all_planners" constraints="{constraints}"/>
+            <SubTree
+              ID="Move to Waypoint"
+              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+              joint_group_name="manipulator"
+              waypoint_name="Grasp Left"
+              planner_interface="moveit_all_planners"
+              constraints="{constraints}"
+            />
           </Decorator>
         </Control>
       </Control>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/cycle_between_waypoints.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/cycle_between_waypoints.xml
index 4164da10..e9bf21b2 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/cycle_between_waypoints.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/cycle_between_waypoints.xml
@@ -1,11 +1,24 @@
-<?xml version="1.0"?>
-  <root BTCPP_format="4" main_tree_to_execute="Cycle Between Waypoints">
-    <BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure">
-        <Decorator ID="KeepRunningUntilFailure">
-            <Control ID="Sequence">
-                <SubTree ID="Move to Waypoint" waypoint_name="Pick" 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" />
-            </Control>
-        </Decorator>
-    </BehaviorTree>
+<?xml version="1.0" ?>
+<root BTCPP_format="4" main_tree_to_execute="Cycle Between Waypoints">
+  <BehaviorTree
+    ID="Cycle Between Waypoints"
+    _description="Cycles between two waypoints until failure"
+  >
+    <Decorator ID="KeepRunningUntilFailure">
+      <Control ID="Sequence">
+        <SubTree
+          ID="Move to Waypoint"
+          waypoint_name="Pick"
+          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"
+        />
+      </Control>
+    </Decorator>
+  </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/interpolate_to_joint_state.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/interpolate_to_joint_state.xml
index 3adb3dc7..ef1e22e5 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/interpolate_to_joint_state.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/interpolate_to_joint_state.xml
@@ -1,20 +1,58 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <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">
-        <Control ID="Fallback" name="root">
-            <Control ID="Sequence">
-                <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
-                <Action ID="InitializeMTCTask" task_id="interpolate_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_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="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"/>
-            </Control>
-        </Control>
-    </BehaviorTree>
+  <BehaviorTree
+    ID="Interpolate to Joint State"
+    _description="Move to a specified joint state using joint interpolation"
+  >
+    <Control ID="Fallback" name="root">
+      <Control ID="Sequence">
+        <Action
+          ID="RetrieveJointStateParameter"
+          timeout_sec="-1"
+          joint_state="{target_joint_state}"
+        />
+        <Action
+          ID="InitializeMTCTask"
+          task_id="interpolate_to_joint_state"
+          controller_names="/joint_trajectory_controller /robotiq_gripper_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="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" />
+      </Control>
+    </Control>
+  </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/joint_diagnostic.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/joint_diagnostic.xml
index 361fe472..f823ccb7 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/joint_diagnostic.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/joint_diagnostic.xml
@@ -1,13 +1,32 @@
-<?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="Move to a known pose and cycle through the min and max limits of a joint repeatedly" _favorite="true">
+  <BehaviorTree
+    ID="Joint Diagnostic"
+    _description="Move to a known pose and cycle through the min and max limits of a joint repeatedly"
+    _favorite="true"
+  >
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Move to Waypoint" waypoint_name="Home" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" />
+      <SubTree
+        ID="Move to Waypoint"
+        waypoint_name="Home"
+        joint_group_name="manipulator"
+        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+      />
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
-          <SubTree ID="Move to Waypoint" waypoint_name="Wrist 2 Max" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" />
-          <SubTree ID="Move to Waypoint" waypoint_name="Wrist 2 Min" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" />
+          <SubTree
+            ID="Move to Waypoint"
+            waypoint_name="Wrist 2 Max"
+            joint_group_name="manipulator"
+            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+          />
+          <SubTree
+            ID="Move to Waypoint"
+            waypoint_name="Wrist 2 Min"
+            joint_group_name="manipulator"
+            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+          />
         </Control>
       </Decorator>
     </Control>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_along_path.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_along_path.xml
index 9cf2142f..d029f10d 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_along_path.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_along_path.xml
@@ -1,25 +1,103 @@
-<?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" 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"
+            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>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_along_path_admittance.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_along_path_admittance.xml
index a9692485..35ab7337 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_along_path_admittance.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_along_path_admittance.xml
@@ -1,28 +1,111 @@
-<?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" 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"
+            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>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_joint_state.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_joint_state.xml
index 9a352adf..a8270920 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_joint_state.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_joint_state.xml
@@ -1,20 +1,52 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <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">
-        <Control ID="Fallback" name="root">
-            <Control ID="Sequence">
-                <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
-                <Action ID="InitializeMTCTask" task_id="move_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_waypoint_task}"/>
-                <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
-                <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}"  />
-                <Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
-                <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_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"/>
-            </Control>
-        </Control>
-    </BehaviorTree>
+  <BehaviorTree
+    ID="Move to Joint State"
+    _description="Move to a specified joint state"
+  >
+    <Control ID="Fallback" name="root">
+      <Control ID="Sequence">
+        <Action
+          ID="RetrieveJointStateParameter"
+          timeout_sec="-1"
+          joint_state="{target_joint_state}"
+        />
+        <Action
+          ID="InitializeMTCTask"
+          task_id="move_to_joint_state"
+          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
+          task="{move_to_waypoint_task}"
+        />
+        <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
+        <Action
+          ID="SetupMTCMoveToJointState"
+          joint_state="{target_joint_state}"
+          name="SetupMTCMoveToJointState_First"
+          planning_group_name="manipulator"
+          task="{move_to_waypoint_task}"
+        />
+        <Action
+          ID="PlanMTCTask"
+          solution="{move_to_waypoint_solution}"
+          task="{move_to_waypoint_task}"
+        />
+        <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_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" />
+      </Control>
+    </Control>
+  </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml
index fd8ed037..51a3f183 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml
@@ -1,30 +1,68 @@
-<?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="SetupMTCMoveToPose" 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="SetupMTCMoveToPose"
+          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}" />
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/open_gripper.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/open_gripper.xml
index a05b6179..edf64720 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/open_gripper.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/open_gripper.xml
@@ -1,9 +1,13 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <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"/>
-        </Control>
-    </BehaviorTree>
+  <!-- ////////// -->
+  <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"
+      />
+    </Control>
+  </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml
index 400cf41b..9c4f1ec4 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml
@@ -1,15 +1,23 @@
-<?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"/>
+    <SubTree ID="Re-Zero Force-Torque Sensors" />
   </TreeNodesModel>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/reactivate_gripper.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/reactivate_gripper.xml
index a9666a46..3ba73b6a 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/reactivate_gripper.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/reactivate_gripper.xml
@@ -1,16 +1,19 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <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" />
-        </Control>
-    </BehaviorTree>
-    <!-- ////////// -->
-    <TreeNodesModel>
-        <Action ID="CallTriggerService">
-            <inout_port name="service_name" />
-        </Action>
-    </TreeNodesModel>
-    <!-- ////////// -->
+  <!-- ////////// -->
+  <BehaviorTree ID="Reactivate Gripper" _description="Reactivate the gripper">
+    <Control ID="Sequence" name="root">
+      <Action
+        ID="CallTriggerService"
+        service_name="/robotiq_activation_controller/reactivate_gripper"
+      />
+    </Control>
+  </BehaviorTree>
+  <!-- ////////// -->
+  <TreeNodesModel>
+    <Action ID="CallTriggerService">
+      <inout_port name="service_name" />
+    </Action>
+  </TreeNodesModel>
+  <!-- ////////// -->
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/request_teleoperation.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/request_teleoperation.xml
index bf3d5b7e..90696f74 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/request_teleoperation.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/request_teleoperation.xml
@@ -1,35 +1,54 @@
-<?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 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">
-              <SubTree ID="Interpolate to Joint State"/>
+              <SubTree ID="Interpolate to Joint State" />
             </Decorator>
             <!-- Interactive markers move to pose -->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 4">
-              <SubTree ID="Move to Pose"/>
+              <SubTree ID="Move to Pose" />
             </Decorator>
             <!-- Waypoint buttons move to joint state -->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 3">
-              <SubTree ID="Move to Joint State"/>
+              <SubTree ID="Move to Joint State" />
             </Decorator>
             <!-- Cartesian and joint jogging -->
-            <Action ID="TeleoperateTwist" controller_name="servo_controller" _while="teleop_mode == 2"/>
-            <Action ID="TeleoperateJointJog" controller_name="servo_controller" _while="teleop_mode == 1"/>
+            <Action
+              ID="TeleoperateTwist"
+              controller_name="servo_controller"
+              _while="teleop_mode == 2"
+            />
+            <Action
+              ID="TeleoperateJointJog"
+              controller_name="servo_controller"
+              _while="teleop_mode == 1"
+            />
           </Control>
         </Decorator>
       </Control>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/reset_planning_scene.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/reset_planning_scene.xml
index 8e43ac5a..999da975 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/reset_planning_scene.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/reset_planning_scene.xml
@@ -1,7 +1,15 @@
-<?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="" _favorite="false" _hardcoded="false">
-    <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
+  <BehaviorTree
+    ID="Reset Planning Scene"
+    _description=""
+    _favorite="false"
+    _hardcoded="false"
+  >
+    <Action
+      ID="ResetPlanningSceneObjects"
+      apply_planning_scene_service="/apply_planning_scene"
+    />
   </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml
index 09ddad25..78f8523c 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml
@@ -1,25 +1,61 @@
-<?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" />
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/take_snapshot.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/take_snapshot.xml
index 070f8158..4b1c1bca 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/take_snapshot.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/take_snapshot.xml
@@ -1,12 +1,29 @@
-<?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_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>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/teleoperate.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/teleoperate.xml
index f9b45da4..d230d7ee 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/teleoperate.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/teleoperate.xml
@@ -1,7 +1,17 @@
-<?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." _favorite="true" _hardcoded="false">
-    <SubTree ID="Request Teleoperation" enable_user_interaction="false" user_interaction_prompt="" initial_teleop_mode="3"/>
+  <BehaviorTree
+    ID="Teleoperate"
+    _description="Handles the different variations of teleoperation from the web UI. Can be used standalone."
+    _favorite="true"
+    _hardcoded="false"
+  >
+    <SubTree
+      ID="Request Teleoperation"
+      enable_user_interaction="false"
+      user_interaction_prompt=""
+      initial_teleop_mode="3"
+    />
   </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/test.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/test.xml
index 29e1e3da..eebdaa56 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/test.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/test.xml
@@ -1,13 +1,16 @@
-<?xml version='1.0' encoding='UTF-8'?>
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="test">
   <!--//////////-->
   <BehaviorTree ID="test" _description="" _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="AlwaysSuccess"/>
-      <Action ID="LoadObjectiveParameters" config_file_name="51mm_apriltag_detection_config.yaml"/>
-      <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info"/>
-      <Action ID="GetImage" topic_name="/wrist_camera/color"/>
-      <Action ID="DetectAprilTags" parameters="{parameters}"/>
+      <Action ID="AlwaysSuccess" />
+      <Action
+        ID="LoadObjectiveParameters"
+        config_file_name="51mm_apriltag_detection_config.yaml"
+      />
+      <Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" />
+      <Action ID="GetImage" topic_name="/wrist_camera/color" />
+      <Action ID="DetectAprilTags" parameters="{parameters}" />
     </Control>
   </BehaviorTree>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/velocity_force_controller_test.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/velocity_force_controller_test.xml
index c23743d7..79e136ca 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/velocity_force_controller_test.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/velocity_force_controller_test.xml
@@ -1,14 +1,36 @@
-<?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 Test">
   <!--//////////-->
-  <BehaviorTree ID="Velocity Force Controller Test" _description="" _favorite="true">
+  <BehaviorTree
+    ID="Velocity Force Controller Test"
+    _description=""
+    _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;-2.0" stamped_wrench="{desired_wrench}"/>
-      <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="false"/>
+      <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;-2.0"
+        stamped_wrench="{desired_wrench}"
+      />
+      <SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="false" />
       <Decorator ID="KeepRunningUntilFailure">
-        <Action ID="PublishVelocityForceCommand" wrench_gain="0.1" velocity_controlled_axes="0;0;0;0;0;0" force_controlled_axes="0;0;1;0;0;0"/>
+        <Action
+          ID="PublishVelocityForceCommand"
+          wrench_gain="0.1"
+          velocity_controlled_axes="0;0;0;0;0;0"
+          force_controlled_axes="0;0;1;0;0;0"
+        />
       </Decorator>
     </Control>
   </BehaviorTree>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/velocity_force_controller_zero.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/velocity_force_controller_zero.xml
index abbb369b..73cc3ba7 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/velocity_force_controller_zero.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/velocity_force_controller_zero.xml
@@ -1,13 +1,35 @@
-<?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="" _favorite="true">
+  <BehaviorTree
+    ID="Velocity Force Controller Zero"
+    _description=""
+    _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>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/visual_servo_teach_reference.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/visual_servo_teach_reference.xml
index 8f96994b..4fd3b5e4 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/visual_servo_teach_reference.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/visual_servo_teach_reference.xml
@@ -1,13 +1,52 @@
-<?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="0"/>
-      <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="0"
+      />
+      <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>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/visual_servo_to_reference.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/visual_servo_to_reference.xml
index ffef0f9f..8e7d9c84 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/visual_servo_to_reference.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/visual_servo_to_reference.xml
@@ -1,31 +1,100 @@
-<?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>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/wait_for_trajectory_approval_if_user_available.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/wait_for_trajectory_approval_if_user_available.xml
index 7abb4108..2f64f16a 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/wait_for_trajectory_approval_if_user_available.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/wait_for_trajectory_approval_if_user_available.xml
@@ -1,11 +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>
 </root>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/write_picknik.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/write_picknik.xml
index c773808c..3340ce35 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/write_picknik.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/write_picknik.xml
@@ -1,19 +1,64 @@
-<?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" 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"
+              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>
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/package.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/package.xml
index ae4861d8..5f8a2151 100644
--- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/package.xml
+++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/package.xml
@@ -1,9 +1,11 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <package format="3">
   <name>kinova_gen3_mujoco_config</name>
   <version>5.1.0</version>
 
-  <description>Configuration package for a UR arm that can be simulated with mock hardware</description>
+  <description>
+    Configuration package for a UR arm that can be simulated with mock hardware
+  </description>
 
   <maintainer email="paul.gesel@picknik.ai">Paul Gesel</maintainer>
   <maintainer email="david.yackzan@picknik.ai">David Yackzan</maintainer>
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 54f0feba..aa66e38a 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,9 +1,12 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <?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>
   <version>5.1.0</version>
-  <description>Provides a node to monitor the protective stop state of the Kinova, and reset protective stops when necessary.</description>
+  <description>
+    Provides a node to monitor the protective stop state of the Kinova, and
+    reset protective stops when necessary.
+  </description>
   <maintainer email="lovro.ivanov@picknik.ai">Lovro Ivanov</maintainer>
   <license>Proprietary</license>
 
diff --git a/src/moveit_pro_mobile_manipulation/README.md b/src/moveit_pro_mobile_manipulation/README.md
index 04f9294c..1c2fcf7f 100644
--- a/src/moveit_pro_mobile_manipulation/README.md
+++ b/src/moveit_pro_mobile_manipulation/README.md
@@ -1 +1 @@
-# moveit_pro_mobile_manipulation
\ No newline at end of file
+# moveit_pro_mobile_manipulation
diff --git a/src/moveit_pro_ur_configs/README.md b/src/moveit_pro_ur_configs/README.md
index e359df7d..158f3b50 100644
--- a/src/moveit_pro_ur_configs/README.md
+++ b/src/moveit_pro_ur_configs/README.md
@@ -1 +1 @@
-# MoveIt Pro family of UR configuration packages
\ No newline at end of file
+# MoveIt Pro family of UR configuration packages
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/move_to_waypoint.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/move_to_waypoint.xml
index f98b5775..36f0e3d8 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/move_to_waypoint.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/move_to_waypoint.xml
@@ -1,20 +1,49 @@
 <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">
-        <Control ID="Sequence" name="root">
-            <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}" joint_group_name="{joint_group_name}"/>
-            <Action ID="InitializeMTCTask" task_id="move_to_named_pose" controller_names="{controller_names}" task="{move_to_waypoint_task}"/>
-            <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
-            <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="{joint_group_name}" planner_interface="{planner_interface}" constraints="{constraints}" task="{move_to_waypoint_task}"/>
-            <Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
-            <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
-        </Control>
-    </BehaviorTree>
-    <TreeNodesModel>
-        <SubTree ID="Move to Waypoint">
-            <input_port name="waypoint_name" default="Above Place Cube"/>
-            <input_port name="joint_group_name" default="manipulator"/>
-            <input_port name="planner_interface" default="moveit_default"/>
-            <input_port name="controller_names" default="/joint_trajectory_controller"/>
-        </SubTree>
-    </TreeNodesModel>
-</root>
\ No newline at end of file
+  <BehaviorTree
+    ID="Move to Waypoint"
+    _description="This Objective is used when moving to one of the saved waypoints in your site configuration"
+    _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}"
+      />
+      <Action
+        ID="InitializeMTCTask"
+        task_id="move_to_named_pose"
+        controller_names="{controller_names}"
+        task="{move_to_waypoint_task}"
+      />
+      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
+      <Action
+        ID="SetupMTCMoveToJointState"
+        joint_state="{target_joint_state}"
+        name="SetupMTCMoveToJointState"
+        planning_group_name="{joint_group_name}"
+        planner_interface="{planner_interface}"
+        constraints="{constraints}"
+        task="{move_to_waypoint_task}"
+      />
+      <Action
+        ID="PlanMTCTask"
+        solution="{move_to_waypoint_solution}"
+        task="{move_to_waypoint_task}"
+      />
+      <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Move to Waypoint">
+      <input_port name="waypoint_name" default="Above Place Cube" />
+      <input_port name="joint_group_name" default="manipulator" />
+      <input_port name="planner_interface" default="moveit_default" />
+      <input_port
+        name="controller_names"
+        default="/joint_trajectory_controller"
+      />
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_sim_config/objectives/pick_up_cube.xml b/src/moveit_pro_ur_configs/picknik_ur_sim_config/objectives/pick_up_cube.xml
index e085e131..bf782d26 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_sim_config/objectives/pick_up_cube.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_sim_config/objectives/pick_up_cube.xml
@@ -1,12 +1,12 @@
 <root BTCPP_format="4" main_tree_to_execute="Pick up Cube">
-    <!--//////////-->
-    <BehaviorTree ID="Pick up Cube" _description="" _favorite="true">
-        <Control ID="Sequence" name="TopLevelSequence">
-            <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"/>
-        </Control>
-    </BehaviorTree>
-</root>
\ No newline at end of file
+  <!--//////////-->
+  <BehaviorTree ID="Pick up Cube" _description="" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <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" />
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_sim_config/objectives/record_trajectory.xml b/src/moveit_pro_ur_configs/picknik_ur_sim_config/objectives/record_trajectory.xml
index 1c9d9c3e..e0af5266 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_sim_config/objectives/record_trajectory.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_sim_config/objectives/record_trajectory.xml
@@ -1,18 +1,29 @@
 <root BTCPP_format="4" main_tree_to_execute="Record Trajectory">
-    <!--//////////-->
-    <BehaviorTree ID="Record Trajectory" _description="" _favorite="true">
+  <!--//////////-->
+  <BehaviorTree ID="Record Trajectory" _description="" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="InitializeMTCTask"
+        controller_names="/joint_trajectory_controller"
+      />
+      <Action ID="SetupMTCCurrentState" />
+      <Decorator
+        ID="ForEachString"
+        vector_in="Point A;Point B;Point c"
+        out="{waypoint_name}"
+      >
         <Control ID="Sequence" name="TopLevelSequence">
-            <Action ID="InitializeMTCTask" controller_names="/joint_trajectory_controller"/>
-            <Action ID="SetupMTCCurrentState"/>
-            <Decorator ID="ForEachString" vector_in="Point A;Point B;Point c" out="{waypoint_name}">
-                <Control ID="Sequence" name="TopLevelSequence">
-                    <Action ID="LogMessage" message="{index}"/>
-                    <Action ID="RetrieveWaypoint" joint_group_name="manipulator"/>
-                    <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" planner_interface="moveit_default"/>
-                </Control>
-            </Decorator>
-            <Action ID="PlanMTCTask"/>
-            <Action ID="WaitForUserTrajectoryApproval"/>
+          <Action ID="LogMessage" message="{index}" />
+          <Action ID="RetrieveWaypoint" joint_group_name="manipulator" />
+          <Action
+            ID="SetupMTCMoveToJointState"
+            joint_state="{target_joint_state}"
+            planner_interface="moveit_default"
+          />
         </Control>
-    </BehaviorTree>
-</root>
\ No newline at end of file
+      </Decorator>
+      <Action ID="PlanMTCTask" />
+      <Action ID="WaitForUserTrajectoryApproval" />
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/dsfxghsedf.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/dsfxghsedf.xml
index 16cb1210..1ff39cef 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/dsfxghsedf.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/dsfxghsedf.xml
@@ -1,8 +1,8 @@
 <root BTCPP_format="4" main_tree_to_execute="dsfxghsedf">
-    <!--//////////-->
-    <BehaviorTree ID="dsfxghsedf" _description="" _favorite="false">
-        <Control ID="Sequence" name="TopLevelSequence">
-            <Action ID="AlwaysSuccess"/>
-        </Control>
-    </BehaviorTree>
-</root>
\ No newline at end of file
+  <!--//////////-->
+  <BehaviorTree ID="dsfxghsedf" _description="" _favorite="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="AlwaysSuccess" />
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/dzfgsedr.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/dzfgsedr.xml
index 24635c00..05a9b848 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/dzfgsedr.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/dzfgsedr.xml
@@ -1,10 +1,14 @@
 <root BTCPP_format="4" main_tree_to_execute="dzfgsedr">
-    <!--//////////-->
-    <BehaviorTree ID="dzfgsedr" _description="" _favorite="false">
-        <Control ID="Sequence" name="TopLevelSequence">
-            <Action ID="RetrieveJointStateParameter"/>
-            <Action ID="CreateJointState"/>
-            <SubTree ID="Move to Joint State" _collapsed="false" target_joint_state="{target_joint_state}"/>
-        </Control>
-    </BehaviorTree>
-</root>
\ No newline at end of file
+  <!--//////////-->
+  <BehaviorTree ID="dzfgsedr" _description="" _favorite="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="RetrieveJointStateParameter" />
+      <Action ID="CreateJointState" />
+      <SubTree
+        ID="Move to Joint State"
+        _collapsed="false"
+        target_joint_state="{target_joint_state}"
+      />
+    </Control>
+  </BehaviorTree>
+</root>