From b4d170cf5ed97a7795774d20b10a0e1e539ba802 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 1/5] formatting

---
 .../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 +-
 kinova_gen3_mujoco_config/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 +-
 55 files changed, 3860 insertions(+), 1178 deletions(-)

diff --git a/kinova_gen3_base_config/launch/agent_bridge.launch.xml b/kinova_gen3_base_config/launch/agent_bridge.launch.xml
index 1729a3c..70ee0b3 100644
--- a/kinova_gen3_base_config/launch/agent_bridge.launch.xml
+++ b/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/kinova_gen3_base_config/objectives/3_waypoint_pick_and_place.xml b/kinova_gen3_base_config/objectives/3_waypoint_pick_and_place.xml
index 917be90..64786ab 100644
--- a/kinova_gen3_base_config/objectives/3_waypoint_pick_and_place.xml
+++ b/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/kinova_gen3_base_config/objectives/clear_snapshot.xml b/kinova_gen3_base_config/objectives/clear_snapshot.xml
index 2550ff7..e809585 100644
--- a/kinova_gen3_base_config/objectives/clear_snapshot.xml
+++ b/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/kinova_gen3_base_config/objectives/close_gripper.xml b/kinova_gen3_base_config/objectives/close_gripper.xml
index 10ae314..60d18f8 100644
--- a/kinova_gen3_base_config/objectives/close_gripper.xml
+++ b/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/kinova_gen3_base_config/objectives/cycle_between_waypoints.xml b/kinova_gen3_base_config/objectives/cycle_between_waypoints.xml
index 4164da1..e9bf21b 100644
--- a/kinova_gen3_base_config/objectives/cycle_between_waypoints.xml
+++ b/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/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml b/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml
index 06d40f3..9be4f85 100644
--- a/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml
+++ b/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/kinova_gen3_base_config/objectives/interpolate_to_joint_state.xml b/kinova_gen3_base_config/objectives/interpolate_to_joint_state.xml
index 3adb3dc..ef1e22e 100644
--- a/kinova_gen3_base_config/objectives/interpolate_to_joint_state.xml
+++ b/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/kinova_gen3_base_config/objectives/move_along_path.xml b/kinova_gen3_base_config/objectives/move_along_path.xml
index 4444e05..802809c 100644
--- a/kinova_gen3_base_config/objectives/move_along_path.xml
+++ b/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/kinova_gen3_base_config/objectives/move_to_joint_state.xml b/kinova_gen3_base_config/objectives/move_to_joint_state.xml
index 9a352ad..a827092 100644
--- a/kinova_gen3_base_config/objectives/move_to_joint_state.xml
+++ b/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/kinova_gen3_base_config/objectives/move_to_pose.xml b/kinova_gen3_base_config/objectives/move_to_pose.xml
index a52c754..24d83b6 100644
--- a/kinova_gen3_base_config/objectives/move_to_pose.xml
+++ b/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/kinova_gen3_base_config/objectives/move_to_waypoint.xml b/kinova_gen3_base_config/objectives/move_to_waypoint.xml
index eb29902..dd1dac8 100644
--- a/kinova_gen3_base_config/objectives/move_to_waypoint.xml
+++ b/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/kinova_gen3_base_config/objectives/open_gripper.xml b/kinova_gen3_base_config/objectives/open_gripper.xml
index a05b617..edf6472 100644
--- a/kinova_gen3_base_config/objectives/open_gripper.xml
+++ b/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/kinova_gen3_base_config/objectives/reactivate_gripper.xml b/kinova_gen3_base_config/objectives/reactivate_gripper.xml
index a9666a4..3ba73b6 100644
--- a/kinova_gen3_base_config/objectives/reactivate_gripper.xml
+++ b/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/kinova_gen3_base_config/objectives/request_teleoperation.xml b/kinova_gen3_base_config/objectives/request_teleoperation.xml
index bf3d5b7..90696f7 100644
--- a/kinova_gen3_base_config/objectives/request_teleoperation.xml
+++ b/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/kinova_gen3_base_config/objectives/reset_plannning_scene.xml b/kinova_gen3_base_config/objectives/reset_plannning_scene.xml
index 8e43ac5..999da97 100644
--- a/kinova_gen3_base_config/objectives/reset_plannning_scene.xml
+++ b/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/kinova_gen3_base_config/objectives/teleoperate.xml b/kinova_gen3_base_config/objectives/teleoperate.xml
index f9b45da..d230d7e 100644
--- a/kinova_gen3_base_config/objectives/teleoperate.xml
+++ b/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/kinova_gen3_base_config/objectives/wait_for_trajectory_approval_if_user_available.xml b/kinova_gen3_base_config/objectives/wait_for_trajectory_approval_if_user_available.xml
index 7abb410..2f64f16 100644
--- a/kinova_gen3_base_config/objectives/wait_for_trajectory_approval_if_user_available.xml
+++ b/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/kinova_gen3_base_config/package.xml b/kinova_gen3_base_config/package.xml
index bbde377..ea20cb2 100644
--- a/kinova_gen3_base_config/package.xml
+++ b/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/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof.xml b/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof.xml
index 0ce558f..5267290 100644
--- a/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof.xml
+++ b/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/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof_rafti_fingers.xml b/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof_rafti_fingers.xml
index f010402..2f220b1 100644
--- a/kinova_gen3_mujoco_config/description/mujoco/gen3_7dof_rafti_fingers.xml
+++ b/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/kinova_gen3_mujoco_config/description/mujoco/landsat_scene.xml b/kinova_gen3_mujoco_config/description/mujoco/landsat_scene.xml
index 45ede67..987fcaf 100644
--- a/kinova_gen3_mujoco_config/description/mujoco/landsat_scene.xml
+++ b/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/kinova_gen3_mujoco_config/description/mujoco/rafti_fingers_scene.xml b/kinova_gen3_mujoco_config/description/mujoco/rafti_fingers_scene.xml
index 946ea60..8067863 100644
--- a/kinova_gen3_mujoco_config/description/mujoco/rafti_fingers_scene.xml
+++ b/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/kinova_gen3_mujoco_config/description/mujoco/scene.xml b/kinova_gen3_mujoco_config/description/mujoco/scene.xml
index df3736b..1134c64 100644
--- a/kinova_gen3_mujoco_config/description/mujoco/scene.xml
+++ b/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/kinova_gen3_mujoco_config/launch/agent_bridge.launch.xml b/kinova_gen3_mujoco_config/launch/agent_bridge.launch.xml
index 40edf98..4ca979b 100644
--- a/kinova_gen3_mujoco_config/launch/agent_bridge.launch.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/april_tag_object_registration.xml b/kinova_gen3_mujoco_config/objectives/april_tag_object_registration.xml
index 53b7be3..89beb80 100644
--- a/kinova_gen3_mujoco_config/objectives/april_tag_object_registration.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/clear_snapshot.xml b/kinova_gen3_mujoco_config/objectives/clear_snapshot.xml
index 2550ff7..e809585 100644
--- a/kinova_gen3_mujoco_config/objectives/clear_snapshot.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/close_gripper.xml b/kinova_gen3_mujoco_config/objectives/close_gripper.xml
index a8807a3..7d0fa97 100644
--- a/kinova_gen3_mujoco_config/objectives/close_gripper.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/compliant_grasp_landsat.xml b/kinova_gen3_mujoco_config/objectives/compliant_grasp_landsat.xml
index b07be58..ce7e252 100644
--- a/kinova_gen3_mujoco_config/objectives/compliant_grasp_landsat.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml b/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml
index e2ca7db..f795d79 100644
--- a/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml b/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml
index 15a7a3c..758573d 100644
--- a/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/constrained_pick_place.xml b/kinova_gen3_mujoco_config/objectives/constrained_pick_place.xml
index 9b0b842..063de3b 100644
--- a/kinova_gen3_mujoco_config/objectives/constrained_pick_place.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/cycle_between_waypoints.xml b/kinova_gen3_mujoco_config/objectives/cycle_between_waypoints.xml
index 4164da1..e9bf21b 100644
--- a/kinova_gen3_mujoco_config/objectives/cycle_between_waypoints.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/interpolate_to_joint_state.xml b/kinova_gen3_mujoco_config/objectives/interpolate_to_joint_state.xml
index 3adb3dc..ef1e22e 100644
--- a/kinova_gen3_mujoco_config/objectives/interpolate_to_joint_state.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/joint_diagnostic.xml b/kinova_gen3_mujoco_config/objectives/joint_diagnostic.xml
index 361fe47..f823ccb 100644
--- a/kinova_gen3_mujoco_config/objectives/joint_diagnostic.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/move_along_path.xml b/kinova_gen3_mujoco_config/objectives/move_along_path.xml
index 9cf2142..d029f10 100644
--- a/kinova_gen3_mujoco_config/objectives/move_along_path.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/move_along_path_admittance.xml b/kinova_gen3_mujoco_config/objectives/move_along_path_admittance.xml
index a969248..35ab733 100644
--- a/kinova_gen3_mujoco_config/objectives/move_along_path_admittance.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/move_to_joint_state.xml b/kinova_gen3_mujoco_config/objectives/move_to_joint_state.xml
index 9a352ad..a827092 100644
--- a/kinova_gen3_mujoco_config/objectives/move_to_joint_state.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/move_to_pose.xml b/kinova_gen3_mujoco_config/objectives/move_to_pose.xml
index fd8ed03..51a3f18 100644
--- a/kinova_gen3_mujoco_config/objectives/move_to_pose.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/open_gripper.xml b/kinova_gen3_mujoco_config/objectives/open_gripper.xml
index a05b617..edf6472 100644
--- a/kinova_gen3_mujoco_config/objectives/open_gripper.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml b/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml
index 400cf41..9c4f1ec 100644
--- a/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/reactivate_gripper.xml b/kinova_gen3_mujoco_config/objectives/reactivate_gripper.xml
index a9666a4..3ba73b6 100644
--- a/kinova_gen3_mujoco_config/objectives/reactivate_gripper.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/request_teleoperation.xml b/kinova_gen3_mujoco_config/objectives/request_teleoperation.xml
index bf3d5b7..90696f7 100644
--- a/kinova_gen3_mujoco_config/objectives/request_teleoperation.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/reset_planning_scene.xml b/kinova_gen3_mujoco_config/objectives/reset_planning_scene.xml
index 8e43ac5..999da97 100644
--- a/kinova_gen3_mujoco_config/objectives/reset_planning_scene.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml b/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml
index 09ddad2..78f8523 100644
--- a/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/take_snapshot.xml b/kinova_gen3_mujoco_config/objectives/take_snapshot.xml
index 070f815..4b1c1bc 100644
--- a/kinova_gen3_mujoco_config/objectives/take_snapshot.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/teleoperate.xml b/kinova_gen3_mujoco_config/objectives/teleoperate.xml
index f9b45da..d230d7e 100644
--- a/kinova_gen3_mujoco_config/objectives/teleoperate.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/test.xml b/kinova_gen3_mujoco_config/objectives/test.xml
index 29e1e3d..eebdaa5 100644
--- a/kinova_gen3_mujoco_config/objectives/test.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/velocity_force_controller_test.xml b/kinova_gen3_mujoco_config/objectives/velocity_force_controller_test.xml
index c23743d..79e136c 100644
--- a/kinova_gen3_mujoco_config/objectives/velocity_force_controller_test.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/velocity_force_controller_zero.xml b/kinova_gen3_mujoco_config/objectives/velocity_force_controller_zero.xml
index abbb369..73cc3ba 100644
--- a/kinova_gen3_mujoco_config/objectives/velocity_force_controller_zero.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/visual_servo_teach_reference.xml b/kinova_gen3_mujoco_config/objectives/visual_servo_teach_reference.xml
index 8f96994..4fd3b5e 100644
--- a/kinova_gen3_mujoco_config/objectives/visual_servo_teach_reference.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/visual_servo_to_reference.xml b/kinova_gen3_mujoco_config/objectives/visual_servo_to_reference.xml
index ffef0f9..8e7d9c8 100644
--- a/kinova_gen3_mujoco_config/objectives/visual_servo_to_reference.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/wait_for_trajectory_approval_if_user_available.xml b/kinova_gen3_mujoco_config/objectives/wait_for_trajectory_approval_if_user_available.xml
index 7abb410..2f64f16 100644
--- a/kinova_gen3_mujoco_config/objectives/wait_for_trajectory_approval_if_user_available.xml
+++ b/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/kinova_gen3_mujoco_config/objectives/write_picknik.xml b/kinova_gen3_mujoco_config/objectives/write_picknik.xml
index c773808..3340ce3 100644
--- a/kinova_gen3_mujoco_config/objectives/write_picknik.xml
+++ b/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/kinova_gen3_mujoco_config/package.xml b/kinova_gen3_mujoco_config/package.xml
index ae4861d..5f8a215 100644
--- a/kinova_gen3_mujoco_config/package.xml
+++ b/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/moveit_studio_kinova_pstop_manager/package.xml b/moveit_studio_kinova_pstop_manager/package.xml
index 54f0feb..aa66e38 100644
--- a/moveit_studio_kinova_pstop_manager/package.xml
+++ b/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>
 

From 171c07254e7a911044bf839cee6ed83d1b542db1 Mon Sep 17 00:00:00 2001
From: Michael Wrock <mike.wrock@gmail.com>
Date: Thu, 14 Nov 2024 16:45:44 -0800
Subject: [PATCH 2/5] version bump

---
 kinova_gen3_base_config/package.xml            | 2 +-
 kinova_gen3_mujoco_config/package.xml          | 2 +-
 moveit_studio_kinova_pstop_manager/package.xml | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/kinova_gen3_base_config/package.xml b/kinova_gen3_base_config/package.xml
index ea20cb2..dcee35f 100644
--- a/kinova_gen3_base_config/package.xml
+++ b/kinova_gen3_base_config/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0" ?>
 <package format="3">
   <name>kinova_gen3_base_config</name>
-  <version>5.1.0</version>
+  <version>6.5.0</version>
 
   <description>Base config package for a Kinova Gen3 system</description>
 
diff --git a/kinova_gen3_mujoco_config/package.xml b/kinova_gen3_mujoco_config/package.xml
index 5f8a215..23df17c 100644
--- a/kinova_gen3_mujoco_config/package.xml
+++ b/kinova_gen3_mujoco_config/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0" ?>
 <package format="3">
   <name>kinova_gen3_mujoco_config</name>
-  <version>5.1.0</version>
+  <version>6.5.0</version>
 
   <description>
     Configuration package for a UR arm that can be simulated with mock hardware
diff --git a/moveit_studio_kinova_pstop_manager/package.xml b/moveit_studio_kinova_pstop_manager/package.xml
index aa66e38..b5031ef 100644
--- a/moveit_studio_kinova_pstop_manager/package.xml
+++ b/moveit_studio_kinova_pstop_manager/package.xml
@@ -2,7 +2,7 @@
 <?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>
+  <version>6.5.0</version>
   <description>
     Provides a node to monitor the protective stop state of the Kinova, and
     reset protective stops when necessary.

From 61d5ed471a4c0ff10516c6b915bce227a7345988 Mon Sep 17 00:00:00 2001
From: Michael Wrock <michael.wrock@picknik.ai>
Date: Mon, 2 Dec 2024 10:58:16 -0800
Subject: [PATCH 3/5] added dependency (#21)

---
 kinova_gen3_base_config/package.xml           |  2 +-
 .../description/picknik_kinova_gen3.xacro     | 32 +++++++++----------
 2 files changed, 17 insertions(+), 17 deletions(-)

diff --git a/kinova_gen3_base_config/package.xml b/kinova_gen3_base_config/package.xml
index dcee35f..40703c6 100644
--- a/kinova_gen3_base_config/package.xml
+++ b/kinova_gen3_base_config/package.xml
@@ -13,8 +13,8 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>moveit_studio_kinova_pstop_manager</depend>
+  <depend>kortex_description</depend>
 
-  <exec_depend>kortex_description</exec_depend>
   <exec_depend>moveit_ros_perception</exec_depend>
   <exec_depend>moveit_studio_agent</exec_depend>
   <exec_depend>moveit_studio_behavior</exec_depend>
diff --git a/kinova_gen3_mujoco_config/description/picknik_kinova_gen3.xacro b/kinova_gen3_mujoco_config/description/picknik_kinova_gen3.xacro
index 3337989..a00a589 100644
--- a/kinova_gen3_mujoco_config/description/picknik_kinova_gen3.xacro
+++ b/kinova_gen3_mujoco_config/description/picknik_kinova_gen3.xacro
@@ -52,7 +52,7 @@
       <visual>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/base_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/base_link.STL"/>
         </geometry>
         <material name="">
           <color rgba="0.75294 0.75294 0.75294 1"/>
@@ -61,7 +61,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/base_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/base_link.STL"/>
         </geometry>
       </collision>
     </link>
@@ -74,7 +74,7 @@
       <visual>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/shoulder_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/shoulder_link.STL"/>
         </geometry>
         <material name="">
           <color rgba="0.75294 0.75294 0.75294 1"/>
@@ -83,7 +83,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/shoulder_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/shoulder_link.STL"/>
         </geometry>
       </collision>
     </link>
@@ -103,7 +103,7 @@
       <visual>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/half_arm_1_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/half_arm_1_link.STL"/>
         </geometry>
         <material name="">
           <color rgba="0.75294 0.75294 0.75294 1"/>
@@ -112,7 +112,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/half_arm_1_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/half_arm_1_link.STL"/>
         </geometry>
       </collision>
     </link>
@@ -132,7 +132,7 @@
       <visual>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/half_arm_2_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/half_arm_2_link.STL"/>
         </geometry>
         <material name="">
           <color rgba="0.75294 0.75294 0.75294 1"/>
@@ -141,7 +141,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/half_arm_2_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/half_arm_2_link.STL"/>
         </geometry>
       </collision>
     </link>
@@ -161,7 +161,7 @@
       <visual>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/forearm_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/forearm_link.STL"/>
         </geometry>
         <material name="">
           <color rgba="0.75294 0.75294 0.75294 1"/>
@@ -170,7 +170,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/forearm_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/forearm_link.STL"/>
         </geometry>
       </collision>
     </link>
@@ -190,7 +190,7 @@
       <visual>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/spherical_wrist_1_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/spherical_wrist_1_link.STL"/>
         </geometry>
         <material name="">
           <color rgba="0.75294 0.75294 0.75294 1"/>
@@ -199,7 +199,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/spherical_wrist_1_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/spherical_wrist_1_link.STL"/>
         </geometry>
       </collision>
     </link>
@@ -219,7 +219,7 @@
       <visual>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/spherical_wrist_2_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/spherical_wrist_2_link.STL"/>
         </geometry>
         <material name="">
           <color rgba="0.75294 0.75294 0.75294 1"/>
@@ -228,7 +228,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/spherical_wrist_2_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/spherical_wrist_2_link.STL"/>
         </geometry>
       </collision>
     </link>
@@ -248,7 +248,7 @@
       <visual>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/bracelet_with_vision_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/bracelet_with_vision_link.STL"/>
         </geometry>
         <material name="">
           <color rgba="0.75294 0.75294 0.75294 1"/>
@@ -257,7 +257,7 @@
       <collision>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/bracelet_with_vision_link.dae"/>
+          <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/bracelet_with_vision_link.STL"/>
         </geometry>
       </collision>
     </link>

From 1fdeeda2deb63d844db852a880589c9f4fda512d Mon Sep 17 00:00:00 2001
From: Michael Wrock <michael.wrock@picknik.ai>
Date: Mon, 2 Dec 2024 22:18:11 -0800
Subject: [PATCH 4/5] Sync V6.5 to main (#29)

* Syncing 6.5 to main
---
 .../description/kortex_robot.xacro            | 97 +++++++++++++++++++
 .../description/picknik_kinova_gen3.xacro     |  2 +-
 .../description/robotiq_2f_85_macro.xacro     | 38 ++++++++
 3 files changed, 136 insertions(+), 1 deletion(-)
 create mode 100644 kinova_gen3_base_config/description/kortex_robot.xacro
 create mode 100644 kinova_gen3_base_config/description/robotiq_2f_85_macro.xacro

diff --git a/kinova_gen3_base_config/description/kortex_robot.xacro b/kinova_gen3_base_config/description/kortex_robot.xacro
new file mode 100644
index 0000000..f0533e2
--- /dev/null
+++ b/kinova_gen3_base_config/description/kortex_robot.xacro
@@ -0,0 +1,97 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="load_robot" params="
+    parent
+    *origin
+    prefix
+    arm
+    gripper
+    gripper_joint_name
+    dof
+    vision
+    robot_ip
+    username
+    password
+    port
+    port_realtime
+    session_inactivity_timeout_ms
+    connection_inactivity_timeout_ms
+    use_internal_bus_gripper_comm:=false
+    use_fake_hardware:=false
+    fake_sensor_commands:=false
+    sim_gazebo:=false
+    sim_ignition:=false
+    sim_isaac:=false
+    isaac_joint_commands:=/isaac_joint_commands
+    isaac_joint_states:=/isaac_joint_states
+    use_external_cable:=false
+    initial_positions:=${dict(joint_1=0.0,joint_2=0.0,joint_3=0.0,joint_4=0.0,joint_5=0.0,joint_6=0.0,joint_7=0.0)}
+    gripper_max_velocity:=100.0
+    gripper_max_force:=100.0
+    gripper_com_port:=/dev/ttyUSB0
+    moveit_active:=false">
+
+    <!-- Include and load arm macro files -->
+    <xacro:include filename="$(find kortex_description)/arms/${arm}/${dof}dof/urdf/${arm}_macro.xacro" />
+
+    <!-- Load the arm -->
+    <xacro:load_arm
+      parent="${parent}"
+      dof="${dof}"
+      vision="${vision}"
+      robot_ip="${robot_ip}"
+      username="${username}"
+      password="${password}"
+      port="${port}"
+      port_realtime="${port_realtime}"
+      session_inactivity_timeout_ms="${session_inactivity_timeout_ms}"
+      connection_inactivity_timeout_ms="${connection_inactivity_timeout_ms}"
+      prefix="${prefix}"
+      use_internal_bus_gripper_comm="${use_internal_bus_gripper_comm}"
+      use_fake_hardware="${use_fake_hardware}"
+      fake_sensor_commands="${fake_sensor_commands}"
+      sim_gazebo="${sim_gazebo}"
+      sim_ignition="${sim_ignition}"
+      sim_isaac="${sim_isaac}"
+      gripper_joint_name="${gripper_joint_name}"
+      gripper_max_velocity="${gripper_max_velocity}"
+      gripper_max_force="${gripper_max_force}"
+      use_external_cable="${use_external_cable}"
+      initial_positions="${initial_positions}">
+      <xacro:insert_block name="origin" />
+    </xacro:load_arm>
+
+    <!-- If no gripper, define tool frame here -->
+    <xacro:if value="${not gripper}">
+      <link name="${prefix}tool_frame"/>
+      <joint name="${prefix}tool_frame_joint" type="fixed">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <parent link="${prefix}${last_arm_link}" />
+        <child link="${prefix}tool_frame" />
+        <axis xyz="0 0 0" />
+      </joint>
+    </xacro:if>
+
+    <!-- Include and load the gripper if defined -->
+    <xacro:unless value="${not gripper}">
+      <xacro:include filename="$(find kinova_gen3_base_config)/description/robotiq_2f_85_macro.xacro" />
+      <!-- last_arm_link is defined in "$(find kortex_description)/arms/${arm}/urdf/${arm}_macro.xacro" -->
+      <xacro:load_gripper
+        parent="${prefix}${last_arm_link}"
+        prefix="${prefix}"
+        use_fake_hardware="${use_fake_hardware}"
+        fake_sensor_commands="${fake_sensor_commands}"
+        sim_ignition="${sim_ignition}"
+        sim_isaac="${sim_isaac}"
+        isaac_joint_commands="${isaac_joint_commands}"
+        isaac_joint_states="${isaac_joint_states}"
+        com_port="${gripper_com_port}"
+        use_internal_bus_gripper_comm="${use_internal_bus_gripper_comm}"
+        moveit_active="${moveit_active}"/>
+    </xacro:unless>
+
+  </xacro:macro>
+
+</robot>
diff --git a/kinova_gen3_base_config/description/picknik_kinova_gen3.xacro b/kinova_gen3_base_config/description/picknik_kinova_gen3.xacro
index 57f5aaa..fdb64cd 100644
--- a/kinova_gen3_base_config/description/picknik_kinova_gen3.xacro
+++ b/kinova_gen3_base_config/description/picknik_kinova_gen3.xacro
@@ -26,7 +26,7 @@
     <xacro:arg name="wrist_realsense" default="true" />
 
     <!-- Import macros for main hardware components -->
-    <xacro:include filename="$(find kortex_description)/robots/kortex_robot.xacro" />
+    <xacro:include filename="$(find kinova_gen3_base_config)/description/kortex_robot.xacro" />
     <xacro:include filename="$(find picknik_accessories)/descriptions/sensors/realsense_d415.urdf.xacro"/>
 
     <!-- Import environment macros -->
diff --git a/kinova_gen3_base_config/description/robotiq_2f_85_macro.xacro b/kinova_gen3_base_config/description/robotiq_2f_85_macro.xacro
new file mode 100644
index 0000000..3f7e631
--- /dev/null
+++ b/kinova_gen3_base_config/description/robotiq_2f_85_macro.xacro
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<robot name="robotiq_2f_85_model" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="load_gripper" params="
+    parent
+    prefix
+    use_fake_hardware:=false
+    fake_sensor_commands:=false
+    sim_ignition:=false
+    sim_isaac:=false
+    isaac_joint_commands:=/isaac_joint_commands
+    isaac_joint_states:=/isaac_joint_states
+    use_internal_bus_gripper_comm:=false
+    com_port:=/dev/ttyUSB0
+    moveit_active:=false">
+    <xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_85_macro.urdf.xacro" />
+
+    <!-- Hardware talks directly to the gripper so we don't need ros2_control unless we are simulating -->
+    <xacro:property name="include_ros2_control" value="false"/>
+    <xacro:if value="${sim_ignition or sim_isaac or use_fake_hardware or not use_internal_bus_gripper_comm}">
+      <xacro:property name="include_ros2_control" value="true"/>
+    </xacro:if>
+
+    <xacro:robotiq_gripper
+        name="RobotiqGripperHardwareInterface"
+        prefix="${prefix}"
+        parent="${parent}"
+        include_ros2_control="${include_ros2_control}"
+        com_port="${com_port}"
+        use_fake_hardware="${use_fake_hardware}"
+        mock_sensor_commands="${fake_sensor_commands}"
+        sim_ignition="${sim_ignition}"
+        sim_isaac="${sim_isaac}"
+        isaac_joint_commands="${isaac_joint_commands}"
+        isaac_joint_states="${isaac_joint_states}">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:robotiq_gripper>
+  </xacro:macro>
+</robot>

From b3c8f59fce40a67ea0cb31bc5e0fda28569d29fc Mon Sep 17 00:00:00 2001
From: Michael Wrock <michael.wrock@picknik.ai>
Date: Thu, 5 Dec 2024 09:34:23 -0800
Subject: [PATCH 5/5] Sync 6.5 to main (#37)

---
 .../objectives/get_imarker_pose_from_mesh_visualization.xml | 3 +++
 kinova_gen3_base_config/objectives/move_to_waypoint.xml     | 3 +++
 kinova_gen3_mujoco_config/objectives/move_to_pose.xml       | 3 +++
 kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml   | 3 +++
 .../objectives/re_zero_force_torque_sensors.xml             | 6 +++++-
 kinova_gen3_mujoco_config/objectives/sample_april_tag.xml   | 3 +++
 6 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml b/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml
index 9be4f85..70fa45b 100644
--- a/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml
+++ b/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml
@@ -26,6 +26,9 @@
       <input_port name="mesh_path" default="{mesh_path}" />
       <input_port name="mesh_pose" default="{mesh_pose}" />
       <input_port name="_collapsed" default="false" />
+      <MetadataFields>
+        <Metadata subcategory="User Input" />
+      </MetadataFields>
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/kinova_gen3_base_config/objectives/move_to_waypoint.xml b/kinova_gen3_base_config/objectives/move_to_waypoint.xml
index dd1dac8..15132ea 100644
--- a/kinova_gen3_base_config/objectives/move_to_waypoint.xml
+++ b/kinova_gen3_base_config/objectives/move_to_waypoint.xml
@@ -38,6 +38,9 @@
   <TreeNodesModel>
     <SubTree ID="Move to Waypoint">
       <input_port name="planner_interface" default="moveit_default" />
+      <MetadataFields>
+        <Metadata subcategory="Motion - Execute" />
+      </MetadataFields>
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/kinova_gen3_mujoco_config/objectives/move_to_pose.xml b/kinova_gen3_mujoco_config/objectives/move_to_pose.xml
index 51a3f18..fca077c 100644
--- a/kinova_gen3_mujoco_config/objectives/move_to_pose.xml
+++ b/kinova_gen3_mujoco_config/objectives/move_to_pose.xml
@@ -63,6 +63,9 @@
       <input_port name="planning_group_name" default="manipulator" />
       <input_port name="ik_frame" default="grasp_link" />
       <input_port name="target_pose" default="{hole_object}" />
+      <MetadataFields>
+        <Metadata subcategory="Motion - Execute" />
+      </MetadataFields>
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml b/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml
index 4733f81..c81b548 100644
--- a/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml
+++ b/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml
@@ -45,6 +45,9 @@
         name="controller_names"
         default="/joint_trajectory_controller"
       />
+      <MetadataFields>
+        <Metadata subcategory="Motion - Execute" />
+      </MetadataFields>
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml b/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml
index 9c4f1ec..5fbbf52 100644
--- a/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml
+++ b/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml
@@ -18,6 +18,10 @@
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Re-Zero Force-Torque Sensors" />
+    <SubTree ID="Re-Zero Force-Torque Sensors">
+      <MetadataFields>
+        <Metadata subcategory="Motion - Execute" />
+      </MetadataFields>
+    </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml b/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml
index 78f8523..0f26bbb 100644
--- a/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml
+++ b/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml
@@ -56,6 +56,9 @@
   <TreeNodesModel>
     <SubTree ID="Sample April Tag">
       <input_port name="tag_id" default="-1" />
+      <MetadataFields>
+        <Metadata subcategory="Perception - Image Processing" />
+      </MetadataFields>
     </SubTree>
   </TreeNodesModel>
 </root>