diff --git a/Dockerfile b/Dockerfile
index 9738b9d..5355ec6 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -117,4 +117,4 @@ ENV USER_WS=${USER_WS}
 WORKDIR $USER_WS
 
 # Set up the user's .bashrc file and shell.
-CMD ["/usr/bin/bash"]
\ No newline at end of file
+CMD ["/usr/bin/bash"]
diff --git a/docker-compose.yaml b/docker-compose.yaml
index 32b5ccd..ce6e089 100644
--- a/docker-compose.yaml
+++ b/docker-compose.yaml
@@ -25,4 +25,4 @@ services:
   dev:
     volumes:
       # Allow access to host hardware e.g. RealSense cameras
-      - /dev:/dev
\ No newline at end of file
+      - /dev:/dev
diff --git a/src/example_robot_hw/launch/additional_driver_nodes.launch.py b/src/example_robot_hw/launch/additional_driver_nodes.launch.py
index 44dd936..e304c8a 100644
--- a/src/example_robot_hw/launch/additional_driver_nodes.launch.py
+++ b/src/example_robot_hw/launch/additional_driver_nodes.launch.py
@@ -82,4 +82,4 @@ def generate_launch_description():
         protective_stop_manager_node,
     ]
 
-    return LaunchDescription(nodes_to_launch)
\ No newline at end of file
+    return LaunchDescription(nodes_to_launch)
diff --git a/src/example_robot_hw/launch/agent_bridge.launch.xml b/src/example_robot_hw/launch/agent_bridge.launch.xml
index 9fa71a0..c922d0a 100644
--- a/src/example_robot_hw/launch/agent_bridge.launch.xml
+++ b/src/example_robot_hw/launch/agent_bridge.launch.xml
@@ -6,4 +6,4 @@
   <include
     file="$(find-pkg-share example_robot_hw)/launch/camera_driver.launch.py"
   />
-</launch>
\ No newline at end of file
+</launch>
diff --git a/src/example_robot_hw/launch/camera_driver.launch.xml b/src/example_robot_hw/launch/camera_driver.launch.xml
index b74c578..a67e762 100644
--- a/src/example_robot_hw/launch/camera_driver.launch.xml
+++ b/src/example_robot_hw/launch/camera_driver.launch.xml
@@ -6,4 +6,4 @@
     <arg name="scene_camera_serial_no" value="'11111111'" />
     <arg name="wrist_mounted_camera_serial_no" value="'11111111'" />
   </include>
-</launch>
\ No newline at end of file
+</launch>
diff --git a/src/example_robot_hw/launch/rs_cameras.launch.xml b/src/example_robot_hw/launch/rs_cameras.launch.xml
index a31b1a6..285ff31 100644
--- a/src/example_robot_hw/launch/rs_cameras.launch.xml
+++ b/src/example_robot_hw/launch/rs_cameras.launch.xml
@@ -71,4 +71,4 @@
       value="/wrist_mounted_camera/depth_registered/image_rect"
     />
   </include>
-</launch>
\ No newline at end of file
+</launch>
diff --git a/src/example_robot_hw/package.xml b/src/example_robot_hw/package.xml
index bb864b1..508fd98 100644
--- a/src/example_robot_hw/package.xml
+++ b/src/example_robot_hw/package.xml
@@ -4,10 +4,8 @@
   <version>0.0.1</version>
   <description>My robot configuration package</description>
 
-  <maintainer email="support@picknik.ai">
-    MoveIt Pro User
-  </maintainer>
-  <license>Apache-2.0</license>
+  <maintainer email="support@picknik.ai">MoveIt Pro User</maintainer>
+  <license>BSD-3-Clause</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>example_robot_sim</depend>
diff --git a/src/example_robot_mock/config/moveit/joint_jog.yaml b/src/example_robot_mock/config/moveit/joint_jog.yaml
index 12ab500..6d80c7e 100644
--- a/src/example_robot_mock/config/moveit/joint_jog.yaml
+++ b/src/example_robot_mock/config/moveit/joint_jog.yaml
@@ -1,4 +1,4 @@
 # Planning groups to use in JointJog, and their corresponding JVC controllers.
 # The number of elements in `planning_groups` and `controllers` must match.
 planning_groups: ['manipulator']
-controllers: ['joint_velocity_controller']
\ No newline at end of file
+controllers: ['joint_velocity_controller']
diff --git a/src/example_robot_mock/config/moveit/joint_limits.yaml b/src/example_robot_mock/config/moveit/joint_limits.yaml
index 21e94f0..4b011f4 100644
--- a/src/example_robot_mock/config/moveit/joint_limits.yaml
+++ b/src/example_robot_mock/config/moveit/joint_limits.yaml
@@ -1,6 +1,6 @@
-# joint_limits.yaml allows overriding per-joint URDF values used for motion planning and speicyfing additional dynamic limits.
-# - Positions limits can be overriden `has_position_limits`, `min_position` and `max_position`.
-# - Velocity limits can be overriden with `has_velocity_limits` and `max_velocity`.
+# joint_limits.yaml allows overriding per-joint URDF values used for motion planning and specifying additional dynamic limits.
+# - Positions limits can be overridden `has_position_limits`, `min_position` and `max_position`.
+# - Velocity limits can be overridden with `has_velocity_limits` and `max_velocity`.
 # - Acceleration limits can be set with `has_acceleration_limits` and `max_acceleration`.
 # - Jerk limits can be set with `has_jerk_limits` and `max_jerk`.
 joint_limits:
diff --git a/src/example_robot_mock/objectives/close_gripper.xml b/src/example_robot_mock/objectives/close_gripper.xml
index 3271f4e..6960f3d 100644
--- a/src/example_robot_mock/objectives/close_gripper.xml
+++ b/src/example_robot_mock/objectives/close_gripper.xml
@@ -25,4 +25,4 @@
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
-</root>
\ No newline at end of file
+</root>
diff --git a/src/example_robot_mock/objectives/open_gripper.xml b/src/example_robot_mock/objectives/open_gripper.xml
index 92a9c1e..a0509e5 100644
--- a/src/example_robot_mock/objectives/open_gripper.xml
+++ b/src/example_robot_mock/objectives/open_gripper.xml
@@ -22,4 +22,4 @@
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
-</root>
\ No newline at end of file
+</root>
diff --git a/src/example_robot_mock/package.xml b/src/example_robot_mock/package.xml
index a488d40..4360e94 100644
--- a/src/example_robot_mock/package.xml
+++ b/src/example_robot_mock/package.xml
@@ -5,7 +5,7 @@
   <description>Example of a mock robot</description>
 
   <maintainer email="support@picknik.ai">MoveIt Pro User</maintainer>
-  <license>Apache-2.0</license>
+  <license>BSD-3-Clause</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <export>
diff --git a/src/example_robot_sim/config/config.yaml b/src/example_robot_sim/config/config.yaml
index 64c6d86..7411268 100644
--- a/src/example_robot_sim/config/config.yaml
+++ b/src/example_robot_sim/config/config.yaml
@@ -25,4 +25,4 @@ ros2_control:
     - "joint_state_broadcaster"
     - "force_torque_sensor_broadcaster"
     - "joint_trajectory_admittance_controller"
-    - "robotiq_gripper_controller"
\ No newline at end of file
+    - "robotiq_gripper_controller"
diff --git a/src/example_robot_sim/mjcf/assets/2f85.xml b/src/example_robot_sim/mjcf/assets/2f85.xml
index 6449be5..211609b 100644
--- a/src/example_robot_sim/mjcf/assets/2f85.xml
+++ b/src/example_robot_sim/mjcf/assets/2f85.xml
@@ -1,55 +1,96 @@
 <mujoco model="robotiq_2f85">
-  <compiler angle="radian" meshdir="assets" autolimits="true"/>
+  <compiler angle="radian" meshdir="assets" autolimits="true" />
 
-  <option cone="elliptic" impratio="10"/>
+  <option cone="elliptic" impratio="10" />
 
   <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="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="gray" rgba="0.4627 0.4627 0.4627 1" />
+    <material name="black" rgba="0.149 0.149 0.149 1" />
 
-    <mesh class="2f85" file="base_mount.stl"/>
-    <mesh class="2f85" file="base.stl"/>
-    <mesh class="2f85" file="driver.stl"/>
-    <mesh class="2f85" file="coupler.stl"/>
-    <mesh class="2f85" file="follower.stl"/>
-    <mesh class="2f85" file="pad.stl"/>
-    <mesh class="2f85" file="silicone_pad.stl"/>
-    <mesh class="2f85" file="spring_link.stl"/>
+    <mesh class="2f85" file="base_mount.stl" />
+    <mesh class="2f85" file="base.stl" />
+    <mesh class="2f85" file="driver.stl" />
+    <mesh class="2f85" file="coupler.stl" />
+    <mesh class="2f85" file="follower.stl" />
+    <mesh class="2f85" file="pad.stl" />
+    <mesh class="2f85" file="silicone_pad.stl" />
+    <mesh class="2f85" file="spring_link.stl" />
   </asset>
 
   <default>
     <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="driver">
-        <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="follower">
-        <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="spring_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="coupler">
-        <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"/>
+        <geom type="mesh" group="3" />
         <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>
@@ -57,88 +98,139 @@
 
   <worldbody>
     <body name="base_mount" pos="0 0 0.007" childclass="2f85">
-      <geom class="visual" mesh="base_mount" material="black"/>
-      <geom class="collision" mesh="base_mount"/>
+      <geom class="visual" mesh="base_mount" material="black" />
+      <geom class="collision" mesh="base_mount" />
       <body name="base" pos="0 0 0.0038" quat="1 0 0 -1">
-        <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="base" material="black"/>
-        <geom class="collision" mesh="base"/>
-        <site name="pinch" pos="0 0 0.145" type="sphere" group="5" rgba="0.9 0.9 0.9 1" size="0.005"/>
+        <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="base" material="black" />
+        <geom class="collision" mesh="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="right_driver" 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="right_driver_joint" class="driver"/>
-          <geom class="visual" mesh="driver" material="gray"/>
-          <geom class="collision" mesh="driver"/>
+          <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="right_driver_joint" class="driver" />
+          <geom class="visual" mesh="driver" material="gray" />
+          <geom class="collision" mesh="driver" />
           <body name="right_coupler" 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="right_coupler_joint" class="coupler"/>
-            <geom class="visual" mesh="coupler" material="black"/>
-            <geom class="collision" mesh="coupler"/>
+            <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="right_coupler_joint" class="coupler" />
+            <geom class="visual" mesh="coupler" material="black" />
+            <geom class="collision" mesh="coupler" />
           </body>
         </body>
         <body name="right_spring_link" 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="right_spring_link_joint" class="spring_link"/>
-          <geom class="visual" mesh="spring_link" material="black"/>
-          <geom class="collision" mesh="spring_link"/>
+          <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="right_spring_link_joint" class="spring_link" />
+          <geom class="visual" mesh="spring_link" material="black" />
+          <geom class="collision" mesh="spring_link" />
           <body name="right_follower" 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="right_follower_joint" class="follower"/>
-            <geom class="visual" mesh="follower" material="black"/>
-            <geom class="collision" mesh="follower"/>
+            <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="right_follower_joint" class="follower" />
+            <geom class="visual" mesh="follower" material="black" />
+            <geom class="collision" mesh="follower" />
             <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="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="pad" />
               <body name="right_silicone_pad">
-                <geom class="visual" mesh="silicone_pad" material="black"/>
+                <geom class="visual" mesh="silicone_pad" material="black" />
               </body>
             </body>
           </body>
         </body>
         <!-- Left-hand side 4-bar linkage -->
         <body name="left_driver" 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="driver"/>
-          <geom class="visual" mesh="driver" material="gray"/>
-          <geom class="collision" mesh="driver"/>
+          <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="driver" />
+          <geom class="visual" mesh="driver" material="gray" />
+          <geom class="collision" mesh="driver" />
           <body name="left_coupler" 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="left_coupler_joint" class="coupler"/>
-            <geom class="visual" mesh="coupler" material="black"/>
-            <geom class="collision" mesh="coupler"/>
+            <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="left_coupler_joint" class="coupler" />
+            <geom class="visual" mesh="coupler" material="black" />
+            <geom class="collision" mesh="coupler" />
           </body>
         </body>
         <body name="left_spring_link" 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="left_spring_link_joint" class="spring_link"/>
-          <geom class="visual" mesh="spring_link" material="black"/>
-          <geom class="collision" mesh="spring_link"/>
+          <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="left_spring_link_joint" class="spring_link" />
+          <geom class="visual" mesh="spring_link" material="black" />
+          <geom class="collision" mesh="spring_link" />
           <body name="left_follower" 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="left_follower_joint" class="follower"/>
-            <geom class="visual" mesh="follower" material="black"/>
-            <geom class="collision" mesh="follower"/>
+            <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="left_follower_joint" class="follower" />
+            <geom class="visual" mesh="follower" material="black" />
+            <geom class="collision" mesh="follower" />
             <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="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="pad" />
               <body name="left_silicone_pad">
-                <geom class="visual" mesh="silicone_pad" material="black"/>
+                <geom class="visual" mesh="silicone_pad" material="black" />
               </body>
             </body>
           </body>
@@ -148,12 +240,12 @@
   </worldbody>
 
   <contact>
-    <exclude body1="base" body2="left_driver"/>
-    <exclude body1="base" body2="right_driver"/>
-    <exclude body1="base" body2="left_spring_link"/>
-    <exclude body1="base" body2="right_spring_link"/>
-    <exclude body1="right_coupler" body2="right_follower"/>
-    <exclude body1="left_coupler" body2="left_follower"/>
+    <exclude body1="base" body2="left_driver" />
+    <exclude body1="base" body2="right_driver" />
+    <exclude body1="base" body2="left_spring_link" />
+    <exclude body1="base" body2="right_spring_link" />
+    <exclude body1="right_coupler" body2="right_follower" />
+    <exclude body1="left_coupler" body2="left_follower" />
   </contact>
 
   <!--
@@ -164,16 +256,33 @@
   -->
   <tendon>
     <fixed name="split">
-      <joint joint="right_driver_joint" coef="0.5"/>
-      <joint joint="robotiq_85_left_knuckle_joint" coef="0.5"/>
+      <joint joint="right_driver_joint" coef="0.5" />
+      <joint joint="robotiq_85_left_knuckle_joint" coef="0.5" />
     </fixed>
   </tendon>
 
   <equality>
-    <connect anchor="0 0 0" body1="right_follower" body2="right_coupler" solimp="0.95 0.99 0.001" solref="0.005 1"/>
-    <connect anchor="0 0 0" body1="left_follower" body2="left_coupler" solimp="0.95 0.99 0.001" solref="0.005 1"/>
-    <joint joint1="right_driver_joint" joint2="robotiq_85_left_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="right_follower"
+      body2="right_coupler"
+      solimp="0.95 0.99 0.001"
+      solref="0.005 1"
+    />
+    <connect
+      anchor="0 0 0"
+      body1="left_follower"
+      body2="left_coupler"
+      solimp="0.95 0.99 0.001"
+      solref="0.005 1"
+    />
+    <joint
+      joint1="right_driver_joint"
+      joint2="robotiq_85_left_knuckle_joint"
+      polycoef="0 1 0 0 0"
+      solimp="0.95 0.99 0.001"
+      solref="0.005 1"
+    />
   </equality>
 
   <!--
@@ -185,7 +294,14 @@
     hence scale = 0.8 * 100 / 255
   -->
   <actuator>
-    <general class="2f85" name="robotiq_85_left_knuckle_joint" tendon="split" forcerange="-5 5" ctrlrange="0 0.8"
-      gainprm="100 0 0" biasprm="0 -100 -10"/>
+    <general
+      class="2f85"
+      name="robotiq_85_left_knuckle_joint"
+      tendon="split"
+      forcerange="-5 5"
+      ctrlrange="0 0.8"
+      gainprm="100 0 0"
+      biasprm="0 -100 -10"
+    />
   </actuator>
 </mujoco>
diff --git a/src/example_robot_sim/mjcf/assets/MUJOCO_LOG.TXT b/src/example_robot_sim/mjcf/assets/MUJOCO_LOG.TXT
index 1232613..3ea7395 100644
--- a/src/example_robot_sim/mjcf/assets/MUJOCO_LOG.TXT
+++ b/src/example_robot_sim/mjcf/assets/MUJOCO_LOG.TXT
@@ -3,4 +3,3 @@ WARNING: Nan, Inf or huge value in QACC at DOF 8. The simulation is unstable. Ti
 
 Wed Feb 11 03:26:47 2026
 WARNING: Nan, Inf or huge value in QACC at DOF 8. The simulation is unstable. Time = 0.2240.
-
diff --git a/src/example_robot_sim/mjcf/assets/arm_description.urdf b/src/example_robot_sim/mjcf/assets/arm_description.urdf
index 476e066..0dce77d 100644
--- a/src/example_robot_sim/mjcf/assets/arm_description.urdf
+++ b/src/example_robot_sim/mjcf/assets/arm_description.urdf
@@ -264,4 +264,3 @@
     </gpio>
   </ros2_control>
 </robot>
-
diff --git a/src/example_robot_sim/mjcf/assets/arm_description.xml b/src/example_robot_sim/mjcf/assets/arm_description.xml
index 328f43e..f672fb3 100644
--- a/src/example_robot_sim/mjcf/assets/arm_description.xml
+++ b/src/example_robot_sim/mjcf/assets/arm_description.xml
@@ -1,57 +1,218 @@
-<?xml version="1.0" ?><mujoco model="r6_2f_proto1">
-  <compiler angle="radian"/>
+<?xml version="1.0" ?>
+<mujoco model="r6_2f_proto1">
+  <compiler angle="radian" />
 
   <option integrator="implicitfast">
-    <flag multiccd="enable"/>
+    <flag multiccd="enable" />
   </option>
 
   <asset>
-    <mesh name="link_0" file="link_0.stl"/>
-    <mesh name="link_1" file="link_1.stl"/>
-    <mesh name="link_2" file="link_2.stl"/>
-    <mesh name="link_3" file="link_3.stl"/>
-    <mesh name="link_4" file="link_4.stl"/>
-    <mesh name="link_5" file="link_5.stl"/>
-    <mesh name="link_6" file="link_6.stl"/>
+    <mesh name="link_0" file="link_0.stl" />
+    <mesh name="link_1" file="link_1.stl" />
+    <mesh name="link_2" file="link_2.stl" />
+    <mesh name="link_3" file="link_3.stl" />
+    <mesh name="link_4" file="link_4.stl" />
+    <mesh name="link_5" file="link_5.stl" />
+    <mesh name="link_6" file="link_6.stl" />
   </asset>
 
   <actuator>
-    <position joint="joint_1" name="joint_1" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_2" name="joint_2" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_3" name="joint_3" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_4" name="joint_4" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_5" name="joint_5" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_6" name="joint_6" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-</actuator><worldbody>
+    <position
+      joint="joint_1"
+      name="joint_1"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_2"
+      name="joint_2"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_3"
+      name="joint_3"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_4"
+      name="joint_4"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_5"
+      name="joint_5"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_6"
+      name="joint_6"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+  </actuator>
+  <worldbody>
     <body name="base_link" gravcomp="1">
-      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_0" name="link_0"/>
-      <geom type="mesh" mesh="link_0" name="link_02"/>
+      <geom
+        type="mesh"
+        contype="0"
+        conaffinity="0"
+        group="1"
+        density="0"
+        mesh="link_0"
+        name="link_0"
+      />
+      <geom type="mesh" mesh="link_0" name="link_02" />
       <body name="link_1" pos="0 0 0.061584" gravcomp="1">
-        <joint name="joint_1" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_1" name="link_1"/>
-        <geom type="mesh" mesh="link_1" name="link_12"/>
-        <body name="link_2" pos="-0.101717 0 0.182284" quat="0.683013 -0.183013 -0.683013 0.183013" gravcomp="1">
-          <joint name="joint_2" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_2" name="link_2"/>
-          <geom type="mesh" mesh="link_2" name="link_22"/>
-          <body name="link_3" pos="0.685682 0 0.041861" quat="0 -0.707107 -0.707107 0" gravcomp="1">
-            <joint name="joint_3" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_3" name="link_3"/>
-            <geom type="mesh" mesh="link_3" name="link_32"/>
-            <body name="link_4" pos="0.518777 0 0.067458" quat="0 -0.965926 -0.258819 0" gravcomp="1">
-              <joint name="joint_4" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_4" name="link_4"/>
-              <geom type="mesh" mesh="link_4" name="link_42"/>
-              <body name="link_5" pos="0.112654 0 0.110903" quat="0.5 -0.5 0.5 -0.5" gravcomp="1">
-                <joint name="joint_5" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_5" name="link_5"/>
-                <geom type="mesh" mesh="link_5" name="link_52"/>
-                <body name="link_6" pos="-0.085976 0 0.133436" quat="0.707107 0 -0.707107 0" gravcomp="1">
-                  <joint name="joint_6" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_6" name="link_6"/>
-                  <geom type="mesh" mesh="link_6" name="link_62"/>
+        <joint
+          name="joint_1"
+          pos="0 0 0"
+          axis="0 0 1"
+          range="-3.14159 3.14159"
+          actuatorfrcrange="-1000 1000"
+        />
+        <geom
+          type="mesh"
+          contype="0"
+          conaffinity="0"
+          group="1"
+          density="0"
+          mesh="link_1"
+          name="link_1"
+        />
+        <geom type="mesh" mesh="link_1" name="link_12" />
+        <body
+          name="link_2"
+          pos="-0.101717 0 0.182284"
+          quat="0.683013 -0.183013 -0.683013 0.183013"
+          gravcomp="1"
+        >
+          <joint
+            name="joint_2"
+            pos="0 0 0"
+            axis="0 0 1"
+            range="-3.14159 3.14159"
+            actuatorfrcrange="-1000 1000"
+          />
+          <geom
+            type="mesh"
+            contype="0"
+            conaffinity="0"
+            group="1"
+            density="0"
+            mesh="link_2"
+            name="link_2"
+          />
+          <geom type="mesh" mesh="link_2" name="link_22" />
+          <body
+            name="link_3"
+            pos="0.685682 0 0.041861"
+            quat="0 -0.707107 -0.707107 0"
+            gravcomp="1"
+          >
+            <joint
+              name="joint_3"
+              pos="0 0 0"
+              axis="0 0 1"
+              range="-3.14159 3.14159"
+              actuatorfrcrange="-1000 1000"
+            />
+            <geom
+              type="mesh"
+              contype="0"
+              conaffinity="0"
+              group="1"
+              density="0"
+              mesh="link_3"
+              name="link_3"
+            />
+            <geom type="mesh" mesh="link_3" name="link_32" />
+            <body
+              name="link_4"
+              pos="0.518777 0 0.067458"
+              quat="0 -0.965926 -0.258819 0"
+              gravcomp="1"
+            >
+              <joint
+                name="joint_4"
+                pos="0 0 0"
+                axis="0 0 1"
+                range="-3.14159 3.14159"
+                actuatorfrcrange="-1000 1000"
+              />
+              <geom
+                type="mesh"
+                contype="0"
+                conaffinity="0"
+                group="1"
+                density="0"
+                mesh="link_4"
+                name="link_4"
+              />
+              <geom type="mesh" mesh="link_4" name="link_42" />
+              <body
+                name="link_5"
+                pos="0.112654 0 0.110903"
+                quat="0.5 -0.5 0.5 -0.5"
+                gravcomp="1"
+              >
+                <joint
+                  name="joint_5"
+                  pos="0 0 0"
+                  axis="0 0 1"
+                  range="-3.14159 3.14159"
+                  actuatorfrcrange="-1000 1000"
+                />
+                <geom
+                  type="mesh"
+                  contype="0"
+                  conaffinity="0"
+                  group="1"
+                  density="0"
+                  mesh="link_5"
+                  name="link_5"
+                />
+                <geom type="mesh" mesh="link_5" name="link_52" />
+                <body
+                  name="link_6"
+                  pos="-0.085976 0 0.133436"
+                  quat="0.707107 0 -0.707107 0"
+                  gravcomp="1"
+                >
+                  <joint
+                    name="joint_6"
+                    pos="0 0 0"
+                    axis="0 0 1"
+                    range="-3.14159 3.14159"
+                    actuatorfrcrange="-1000 1000"
+                  />
+                  <geom
+                    type="mesh"
+                    contype="0"
+                    conaffinity="0"
+                    group="1"
+                    density="0"
+                    mesh="link_6"
+                    name="link_6"
+                  />
+                  <geom type="mesh" mesh="link_6" name="link_62" />
                   <body name="ft_frame" gravcomp="1">
-                    <body name="tool0" pos="0 0 0.185" gravcomp="1"/>
+                    <body name="tool0" pos="0 0 0.185" gravcomp="1" />
                   </body>
                 </body>
               </body>
@@ -64,4 +225,4 @@
   <contact>
     <exclude body1="base_link" body2="link_1" />
   </contact>
-</mujoco>
\ No newline at end of file
+</mujoco>
diff --git a/src/example_robot_sim/mjcf/assets/arm_with_gripper_description.xml b/src/example_robot_sim/mjcf/assets/arm_with_gripper_description.xml
index 07970c3..bd6d386 100644
--- a/src/example_robot_sim/mjcf/assets/arm_with_gripper_description.xml
+++ b/src/example_robot_sim/mjcf/assets/arm_with_gripper_description.xml
@@ -1,28 +1,71 @@
-<?xml version="1.0" ?><mujoco model="r6_2f_proto1">
-  <compiler angle="radian"/>
+<?xml version="1.0" ?>
+<mujoco model="r6_2f_proto1">
+  <compiler angle="radian" />
 
   <option integrator="implicitfast">
-    <flag multiccd="enable"/>
+    <flag multiccd="enable" />
   </option>
 
   <asset>
-    <mesh name="link_0" file="link_0.stl"/>
-    <mesh name="link_1" file="link_1.stl"/>
-    <mesh name="link_2" file="link_2.stl"/>
-    <mesh name="link_3" file="link_3.stl"/>
-    <mesh name="link_4" file="link_4.stl"/>
-    <mesh name="link_5" file="link_5.stl"/>
-    <mesh name="link_6" file="link_6.stl"/>
-    <model name="robotiq_2f_85" file="2f85.xml"/>
+    <mesh name="link_0" file="link_0.stl" />
+    <mesh name="link_1" file="link_1.stl" />
+    <mesh name="link_2" file="link_2.stl" />
+    <mesh name="link_3" file="link_3.stl" />
+    <mesh name="link_4" file="link_4.stl" />
+    <mesh name="link_5" file="link_5.stl" />
+    <mesh name="link_6" file="link_6.stl" />
+    <model name="robotiq_2f_85" file="2f85.xml" />
   </asset>
 
   <actuator>
-    <position joint="joint_1" name="joint_1" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_2" name="joint_2" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_3" name="joint_3" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_4" name="joint_4" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_5" name="joint_5" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_6" name="joint_6" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
+    <position
+      joint="joint_1"
+      name="joint_1"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_2"
+      name="joint_2"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_3"
+      name="joint_3"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_4"
+      name="joint_4"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_5"
+      name="joint_5"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_6"
+      name="joint_6"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
   </actuator>
 
   <asset>
@@ -66,41 +109,162 @@
 
   <worldbody>
     <body name="base_link" gravcomp="1">
-      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_0" name="link_0"/>
-      <geom type="mesh" mesh="link_0" name="link_02"/>
+      <geom
+        type="mesh"
+        contype="0"
+        conaffinity="0"
+        group="1"
+        density="0"
+        mesh="link_0"
+        name="link_0"
+      />
+      <geom type="mesh" mesh="link_0" name="link_02" />
       <body name="link_1" pos="0 0 0.061584" gravcomp="1">
-        <joint name="joint_1" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_1" name="link_1"/>
-        <geom type="mesh" mesh="link_1" name="link_12"/>
-        <body name="link_2" pos="-0.101717 0 0.182284" quat="0.683013 -0.183013 -0.683013 0.183013" gravcomp="1">
-          <joint name="joint_2" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_2" name="link_2"/>
-          <geom type="mesh" mesh="link_2" name="link_22"/>
-          <body name="link_3" pos="0.685682 0 0.041861" quat="0 -0.707107 -0.707107 0" gravcomp="1">
-            <joint name="joint_3" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_3" name="link_3"/>
-            <geom type="mesh" mesh="link_3" name="link_32"/>
-            <body name="link_4" pos="0.518777 0 0.067458" quat="0 -0.965926 -0.258819 0" gravcomp="1">
-              <joint name="joint_4" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_4" name="link_4"/>
-              <geom type="mesh" mesh="link_4" name="link_42"/>
-              <body name="link_5" pos="0.112654 0 0.110903" quat="0.5 -0.5 0.5 -0.5" gravcomp="1">
-                <joint name="joint_5" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_5" name="link_5"/>
-                <geom type="mesh" mesh="link_5" name="link_52"/>
-                <body name="link_6" pos="-0.085976 0 0.133436" quat="0.707107 0 -0.707107 0" gravcomp="1">
-                  <joint name="joint_6" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_6" name="link_6"/>
-                  <geom type="mesh" mesh="link_6" name="link_62"/>
+        <joint
+          name="joint_1"
+          pos="0 0 0"
+          axis="0 0 1"
+          range="-3.14159 3.14159"
+          actuatorfrcrange="-1000 1000"
+        />
+        <geom
+          type="mesh"
+          contype="0"
+          conaffinity="0"
+          group="1"
+          density="0"
+          mesh="link_1"
+          name="link_1"
+        />
+        <geom type="mesh" mesh="link_1" name="link_12" />
+        <body
+          name="link_2"
+          pos="-0.101717 0 0.182284"
+          quat="0.683013 -0.183013 -0.683013 0.183013"
+          gravcomp="1"
+        >
+          <joint
+            name="joint_2"
+            pos="0 0 0"
+            axis="0 0 1"
+            range="-3.14159 3.14159"
+            actuatorfrcrange="-1000 1000"
+          />
+          <geom
+            type="mesh"
+            contype="0"
+            conaffinity="0"
+            group="1"
+            density="0"
+            mesh="link_2"
+            name="link_2"
+          />
+          <geom type="mesh" mesh="link_2" name="link_22" />
+          <body
+            name="link_3"
+            pos="0.685682 0 0.041861"
+            quat="0 -0.707107 -0.707107 0"
+            gravcomp="1"
+          >
+            <joint
+              name="joint_3"
+              pos="0 0 0"
+              axis="0 0 1"
+              range="-3.14159 3.14159"
+              actuatorfrcrange="-1000 1000"
+            />
+            <geom
+              type="mesh"
+              contype="0"
+              conaffinity="0"
+              group="1"
+              density="0"
+              mesh="link_3"
+              name="link_3"
+            />
+            <geom type="mesh" mesh="link_3" name="link_32" />
+            <body
+              name="link_4"
+              pos="0.518777 0 0.067458"
+              quat="0 -0.965926 -0.258819 0"
+              gravcomp="1"
+            >
+              <joint
+                name="joint_4"
+                pos="0 0 0"
+                axis="0 0 1"
+                range="-3.14159 3.14159"
+                actuatorfrcrange="-1000 1000"
+              />
+              <geom
+                type="mesh"
+                contype="0"
+                conaffinity="0"
+                group="1"
+                density="0"
+                mesh="link_4"
+                name="link_4"
+              />
+              <geom type="mesh" mesh="link_4" name="link_42" />
+              <body
+                name="link_5"
+                pos="0.112654 0 0.110903"
+                quat="0.5 -0.5 0.5 -0.5"
+                gravcomp="1"
+              >
+                <joint
+                  name="joint_5"
+                  pos="0 0 0"
+                  axis="0 0 1"
+                  range="-3.14159 3.14159"
+                  actuatorfrcrange="-1000 1000"
+                />
+                <geom
+                  type="mesh"
+                  contype="0"
+                  conaffinity="0"
+                  group="1"
+                  density="0"
+                  mesh="link_5"
+                  name="link_5"
+                />
+                <geom type="mesh" mesh="link_5" name="link_52" />
+                <body
+                  name="link_6"
+                  pos="-0.085976 0 0.133436"
+                  quat="0.707107 0 -0.707107 0"
+                  gravcomp="1"
+                >
+                  <joint
+                    name="joint_6"
+                    pos="0 0 0"
+                    axis="0 0 1"
+                    range="-3.14159 3.14159"
+                    actuatorfrcrange="-1000 1000"
+                  />
+                  <geom
+                    type="mesh"
+                    contype="0"
+                    conaffinity="0"
+                    group="1"
+                    density="0"
+                    mesh="link_6"
+                    name="link_6"
+                  />
+                  <geom type="mesh" mesh="link_6" name="link_62" />
                   <body name="ft_frame" gravcomp="1">
                     <site
-                        name="tcp_fts_sensor"
-                        size="0.01"
-                        pos="0 0 0"
-                        quat="-1 1 0 0"
+                      name="tcp_fts_sensor"
+                      size="0.01"
+                      pos="0 0 0"
+                      quat="-1 1 0 0"
                     />
                     <body name="tool0" pos="0 0 0.185" gravcomp="1">
-                      <attach model="robotiq_2f_85" body="base_mount" prefix=""/>
+                      <attach
+                        model="robotiq_2f_85"
+                        body="base_mount"
+                        prefix=""
+                      />
                     </body>
                   </body>
                 </body>
@@ -118,4 +282,4 @@
     <force site="tcp_fts_sensor" />
     <torque site="tcp_fts_sensor" />
   </sensor>
-</mujoco>
\ No newline at end of file
+</mujoco>
diff --git a/src/example_robot_sim/mjcf/assets/robot_description.urdf b/src/example_robot_sim/mjcf/assets/robot_description.urdf
index 6e881f2..9b0d9b5 100644
--- a/src/example_robot_sim/mjcf/assets/robot_description.urdf
+++ b/src/example_robot_sim/mjcf/assets/robot_description.urdf
@@ -517,4 +517,3 @@
     <child link="grasp_link"/>
   </joint>
 </robot>
-
diff --git a/src/example_robot_sim/mjcf/assets/robot_description.xml b/src/example_robot_sim/mjcf/assets/robot_description.xml
index d3e6f93..1d76570 100644
--- a/src/example_robot_sim/mjcf/assets/robot_description.xml
+++ b/src/example_robot_sim/mjcf/assets/robot_description.xml
@@ -1,123 +1,539 @@
-<?xml version="1.0" ?><mujoco model="r6_2f_proto1">
-  <compiler angle="radian"/>
+<?xml version="1.0" ?>
+<mujoco model="r6_2f_proto1">
+  <compiler angle="radian" />
 
   <option integrator="implicitfast">
-    <flag multiccd="enable"/>
+    <flag multiccd="enable" />
   </option>
 
   <asset>
-    <mesh name="link_0" file="link_0.stl"/>
-    <mesh name="link_1" file="link_1.stl"/>
-    <mesh name="link_2" file="link_2.stl"/>
-    <mesh name="link_3" file="link_3.stl"/>
-    <mesh name="link_4" file="link_4.stl"/>
-    <mesh name="link_5" file="link_5.stl"/>
-    <mesh name="link_6" file="link_6.stl"/>
-    <mesh name="robotiq_base" file="robotiq_base.stl"/>
-    <mesh name="left_knuckle" file="left_knuckle.stl"/>
-    <mesh name="right_knuckle" file="right_knuckle.stl"/>
-    <mesh name="left_finger" file="left_finger.stl"/>
-    <mesh name="right_finger" file="right_finger.stl"/>
-    <mesh name="left_inner_knuckle" file="left_inner_knuckle.stl"/>
-    <mesh name="right_inner_knuckle" file="right_inner_knuckle.stl"/>
-    <mesh name="left_finger_tip" file="left_finger_tip.stl"/>
-    <mesh name="right_finger_tip" file="right_finger_tip.stl"/>
+    <mesh name="link_0" file="link_0.stl" />
+    <mesh name="link_1" file="link_1.stl" />
+    <mesh name="link_2" file="link_2.stl" />
+    <mesh name="link_3" file="link_3.stl" />
+    <mesh name="link_4" file="link_4.stl" />
+    <mesh name="link_5" file="link_5.stl" />
+    <mesh name="link_6" file="link_6.stl" />
+    <mesh name="robotiq_base" file="robotiq_base.stl" />
+    <mesh name="left_knuckle" file="left_knuckle.stl" />
+    <mesh name="right_knuckle" file="right_knuckle.stl" />
+    <mesh name="left_finger" file="left_finger.stl" />
+    <mesh name="right_finger" file="right_finger.stl" />
+    <mesh name="left_inner_knuckle" file="left_inner_knuckle.stl" />
+    <mesh name="right_inner_knuckle" file="right_inner_knuckle.stl" />
+    <mesh name="left_finger_tip" file="left_finger_tip.stl" />
+    <mesh name="right_finger_tip" file="right_finger_tip.stl" />
   </asset>
 
   <actuator>
-    <position joint="joint_1" name="joint_1" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_2" name="joint_2" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_3" name="joint_3" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_4" name="joint_4" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_5" name="joint_5" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="joint_6" name="joint_6" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="robotiq_85_left_knuckle_joint" name="robotiq_85_left_knuckle_joint" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="robotiq_85_left_finger_tip_joint" name="robotiq_85_left_finger_tip_joint" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="robotiq_85_right_knuckle_joint" name="robotiq_85_right_knuckle_joint" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="robotiq_85_right_finger_tip_joint" name="robotiq_85_right_finger_tip_joint" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="robotiq_85_left_inner_knuckle_joint" name="robotiq_85_left_inner_knuckle_joint" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-    <position joint="robotiq_85_right_inner_knuckle_joint" name="robotiq_85_right_inner_knuckle_joint" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
-</actuator><worldbody>
+    <position
+      joint="joint_1"
+      name="joint_1"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_2"
+      name="joint_2"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_3"
+      name="joint_3"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_4"
+      name="joint_4"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_5"
+      name="joint_5"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="joint_6"
+      name="joint_6"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="robotiq_85_left_knuckle_joint"
+      name="robotiq_85_left_knuckle_joint"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="robotiq_85_left_finger_tip_joint"
+      name="robotiq_85_left_finger_tip_joint"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="robotiq_85_right_knuckle_joint"
+      name="robotiq_85_right_knuckle_joint"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="robotiq_85_right_finger_tip_joint"
+      name="robotiq_85_right_finger_tip_joint"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="robotiq_85_left_inner_knuckle_joint"
+      name="robotiq_85_left_inner_knuckle_joint"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+    <position
+      joint="robotiq_85_right_inner_knuckle_joint"
+      name="robotiq_85_right_inner_knuckle_joint"
+      ctrlrange="-3.14 3.14"
+      kp="1000"
+      forcelimited="false"
+      dampratio="1"
+    />
+  </actuator>
+  <worldbody>
     <body name="base_link" gravcomp="1">
-      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_0" name="link_0"/>
-      <geom type="mesh" mesh="link_0" name="link_02"/>
+      <geom
+        type="mesh"
+        contype="0"
+        conaffinity="0"
+        group="1"
+        density="0"
+        mesh="link_0"
+        name="link_0"
+      />
+      <geom type="mesh" mesh="link_0" name="link_02" />
       <body name="link_1" pos="0 0 0.061584" gravcomp="1">
-        <joint name="joint_1" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_1" name="link_1"/>
-        <geom type="mesh" mesh="link_1" name="link_12"/>
-        <body name="link_2" pos="-0.101717 0 0.182284" quat="0.683013 -0.183013 -0.683013 0.183013" gravcomp="1">
-          <joint name="joint_2" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_2" name="link_2"/>
-          <geom type="mesh" mesh="link_2" name="link_22"/>
-          <body name="link_3" pos="0.685682 0 0.041861" quat="0 -0.707107 -0.707107 0" gravcomp="1">
-            <joint name="joint_3" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_3" name="link_3"/>
-            <geom type="mesh" mesh="link_3" name="link_32"/>
-            <body name="link_4" pos="0.518777 0 0.067458" quat="0 -0.965926 -0.258819 0" gravcomp="1">
-              <joint name="joint_4" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_4" name="link_4"/>
-              <geom type="mesh" mesh="link_4" name="link_42"/>
-              <body name="link_5" pos="0.112654 0 0.110903" quat="0.5 -0.5 0.5 -0.5" gravcomp="1">
-                <joint name="joint_5" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_5" name="link_5"/>
-                <geom type="mesh" mesh="link_5" name="link_52"/>
-                <body name="link_6" pos="-0.085976 0 0.133436" quat="0.707107 0 -0.707107 0" gravcomp="1">
-                  <joint name="joint_6" pos="0 0 0" axis="0 0 1" range="-3.14159 3.14159" actuatorfrcrange="-1000 1000"/>
-                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="link_6" name="link_6"/>
-                  <geom type="mesh" mesh="link_6" name="link_62"/>
+        <joint
+          name="joint_1"
+          pos="0 0 0"
+          axis="0 0 1"
+          range="-3.14159 3.14159"
+          actuatorfrcrange="-1000 1000"
+        />
+        <geom
+          type="mesh"
+          contype="0"
+          conaffinity="0"
+          group="1"
+          density="0"
+          mesh="link_1"
+          name="link_1"
+        />
+        <geom type="mesh" mesh="link_1" name="link_12" />
+        <body
+          name="link_2"
+          pos="-0.101717 0 0.182284"
+          quat="0.683013 -0.183013 -0.683013 0.183013"
+          gravcomp="1"
+        >
+          <joint
+            name="joint_2"
+            pos="0 0 0"
+            axis="0 0 1"
+            range="-3.14159 3.14159"
+            actuatorfrcrange="-1000 1000"
+          />
+          <geom
+            type="mesh"
+            contype="0"
+            conaffinity="0"
+            group="1"
+            density="0"
+            mesh="link_2"
+            name="link_2"
+          />
+          <geom type="mesh" mesh="link_2" name="link_22" />
+          <body
+            name="link_3"
+            pos="0.685682 0 0.041861"
+            quat="0 -0.707107 -0.707107 0"
+            gravcomp="1"
+          >
+            <joint
+              name="joint_3"
+              pos="0 0 0"
+              axis="0 0 1"
+              range="-3.14159 3.14159"
+              actuatorfrcrange="-1000 1000"
+            />
+            <geom
+              type="mesh"
+              contype="0"
+              conaffinity="0"
+              group="1"
+              density="0"
+              mesh="link_3"
+              name="link_3"
+            />
+            <geom type="mesh" mesh="link_3" name="link_32" />
+            <body
+              name="link_4"
+              pos="0.518777 0 0.067458"
+              quat="0 -0.965926 -0.258819 0"
+              gravcomp="1"
+            >
+              <joint
+                name="joint_4"
+                pos="0 0 0"
+                axis="0 0 1"
+                range="-3.14159 3.14159"
+                actuatorfrcrange="-1000 1000"
+              />
+              <geom
+                type="mesh"
+                contype="0"
+                conaffinity="0"
+                group="1"
+                density="0"
+                mesh="link_4"
+                name="link_4"
+              />
+              <geom type="mesh" mesh="link_4" name="link_42" />
+              <body
+                name="link_5"
+                pos="0.112654 0 0.110903"
+                quat="0.5 -0.5 0.5 -0.5"
+                gravcomp="1"
+              >
+                <joint
+                  name="joint_5"
+                  pos="0 0 0"
+                  axis="0 0 1"
+                  range="-3.14159 3.14159"
+                  actuatorfrcrange="-1000 1000"
+                />
+                <geom
+                  type="mesh"
+                  contype="0"
+                  conaffinity="0"
+                  group="1"
+                  density="0"
+                  mesh="link_5"
+                  name="link_5"
+                />
+                <geom type="mesh" mesh="link_5" name="link_52" />
+                <body
+                  name="link_6"
+                  pos="-0.085976 0 0.133436"
+                  quat="0.707107 0 -0.707107 0"
+                  gravcomp="1"
+                >
+                  <joint
+                    name="joint_6"
+                    pos="0 0 0"
+                    axis="0 0 1"
+                    range="-3.14159 3.14159"
+                    actuatorfrcrange="-1000 1000"
+                  />
+                  <geom
+                    type="mesh"
+                    contype="0"
+                    conaffinity="0"
+                    group="1"
+                    density="0"
+                    mesh="link_6"
+                    name="link_6"
+                  />
+                  <geom type="mesh" mesh="link_6" name="link_62" />
                   <body name="ft_frame" gravcomp="1">
                     <body name="tool0" pos="0 0 0.185" gravcomp="1">
                       <body name="robotiq_85_base_link" gravcomp="1">
-                        <inertial pos="0 2.274e-05 0.0323229" quat="0.707251 -0.000590537 0.000590296 0.706962" mass="0.663202" diaginertia="0.000588023 0.000516178 0.000394627"/>
-                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="robotiq_base" name="robotiq_base"/>
-                        <geom type="mesh" mesh="robotiq_base" name="robotiq_base2"/>
-                        <body name="robotiq_85_left_knuckle_link" pos="0.0306011 0 0.0549045" gravcomp="1">
-                          <inertial pos="0.012132 0.0002 -0.00058647" quat="0.484217 0.5153 0.5153 0.484217" mass="0.0138477" diaginertia="2.31944e-06 2.23867e-06 3.45008e-07"/>
-                          <joint name="robotiq_85_left_knuckle_joint" pos="0 0 0" axis="0 -1 0" range="0 0.8" actuatorfrcrange="-50 50"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="left_knuckle" name="left_knuckle"/>
-                          <geom type="mesh" mesh="left_knuckle" name="left_knuckle2"/>
-                          <body name="robotiq_85_left_finger_link" pos="0.0315262 0 -0.00376347" gravcomp="1">
-                            <inertial pos="0.00346899 -0.00079447 0.0186712" quat="0.99325 0 0.115989 0" mass="0.0426038" diaginertia="1.43724e-05 1.18321e-05 4.68228e-06"/>
-                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="left_finger" name="left_finger"/>
-                            <geom type="mesh" mesh="left_finger" name="left_finger2"/>
-                            <body name="robotiq_85_left_finger_tip_link" pos="0.00563134 0 0.0471852" gravcomp="1">
-                              <inertial pos="-0.0145671 -0.0008 0.016497" quat="0.977773 0 -0.209665 0" mass="0.0426859" diaginertia="1.20653e-05 1.19789e-05 2.64405e-06"/>
-                              <joint name="robotiq_85_left_finger_tip_joint" pos="0 0 0" axis="0 -1 0"/>
-                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="left_finger_tip" name="left_finger_tip"/>
-                              <geom type="mesh" mesh="left_finger_tip" name="left_finger_tip2"/>
+                        <inertial
+                          pos="0 2.274e-05 0.0323229"
+                          quat="0.707251 -0.000590537 0.000590296 0.706962"
+                          mass="0.663202"
+                          diaginertia="0.000588023 0.000516178 0.000394627"
+                        />
+                        <geom
+                          type="mesh"
+                          contype="0"
+                          conaffinity="0"
+                          group="1"
+                          density="0"
+                          mesh="robotiq_base"
+                          name="robotiq_base"
+                        />
+                        <geom
+                          type="mesh"
+                          mesh="robotiq_base"
+                          name="robotiq_base2"
+                        />
+                        <body
+                          name="robotiq_85_left_knuckle_link"
+                          pos="0.0306011 0 0.0549045"
+                          gravcomp="1"
+                        >
+                          <inertial
+                            pos="0.012132 0.0002 -0.00058647"
+                            quat="0.484217 0.5153 0.5153 0.484217"
+                            mass="0.0138477"
+                            diaginertia="2.31944e-06 2.23867e-06 3.45008e-07"
+                          />
+                          <joint
+                            name="robotiq_85_left_knuckle_joint"
+                            pos="0 0 0"
+                            axis="0 -1 0"
+                            range="0 0.8"
+                            actuatorfrcrange="-50 50"
+                          />
+                          <geom
+                            type="mesh"
+                            contype="0"
+                            conaffinity="0"
+                            group="1"
+                            density="0"
+                            mesh="left_knuckle"
+                            name="left_knuckle"
+                          />
+                          <geom
+                            type="mesh"
+                            mesh="left_knuckle"
+                            name="left_knuckle2"
+                          />
+                          <body
+                            name="robotiq_85_left_finger_link"
+                            pos="0.0315262 0 -0.00376347"
+                            gravcomp="1"
+                          >
+                            <inertial
+                              pos="0.00346899 -0.00079447 0.0186712"
+                              quat="0.99325 0 0.115989 0"
+                              mass="0.0426038"
+                              diaginertia="1.43724e-05 1.18321e-05 4.68228e-06"
+                            />
+                            <geom
+                              type="mesh"
+                              contype="0"
+                              conaffinity="0"
+                              group="1"
+                              density="0"
+                              mesh="left_finger"
+                              name="left_finger"
+                            />
+                            <geom
+                              type="mesh"
+                              mesh="left_finger"
+                              name="left_finger2"
+                            />
+                            <body
+                              name="robotiq_85_left_finger_tip_link"
+                              pos="0.00563134 0 0.0471852"
+                              gravcomp="1"
+                            >
+                              <inertial
+                                pos="-0.0145671 -0.0008 0.016497"
+                                quat="0.977773 0 -0.209665 0"
+                                mass="0.0426859"
+                                diaginertia="1.20653e-05 1.19789e-05 2.64405e-06"
+                              />
+                              <joint
+                                name="robotiq_85_left_finger_tip_joint"
+                                pos="0 0 0"
+                                axis="0 -1 0"
+                              />
+                              <geom
+                                type="mesh"
+                                contype="0"
+                                conaffinity="0"
+                                group="1"
+                                density="0"
+                                mesh="left_finger_tip"
+                                name="left_finger_tip"
+                              />
+                              <geom
+                                type="mesh"
+                                mesh="left_finger_tip"
+                                name="left_finger_tip2"
+                              />
                             </body>
                           </body>
                         </body>
-                        <body name="robotiq_85_right_knuckle_link" pos="-0.0306011 0 0.0549045" gravcomp="1">
-                          <inertial pos="-0.012132 0.0002 -0.00058647" quat="0.5153 0.484217 0.484217 0.5153" mass="0.0138477" diaginertia="2.31944e-06 2.23867e-06 3.45008e-07"/>
-                          <joint name="robotiq_85_right_knuckle_joint" pos="0 0 0" axis="0 -1 0" range="-0.8 0" actuatorfrcrange="-50 50"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="right_knuckle" name="right_knuckle"/>
-                          <geom type="mesh" mesh="right_knuckle" name="right_knuckle2"/>
-                          <body name="robotiq_85_right_finger_link" pos="-0.0315262 0 -0.00376347" gravcomp="1">
-                            <inertial pos="-0.00346899 -5.53e-06 0.0186712" quat="0.99325 0 -0.115989 0" mass="0.0426038" diaginertia="1.43724e-05 1.18321e-05 4.68228e-06"/>
-                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="right_finger" name="right_finger"/>
-                            <geom type="mesh" mesh="right_finger" name="right_finger2"/>
-                            <body name="robotiq_85_right_finger_tip_link" pos="-0.00563134 0 0.0471852" gravcomp="1">
-                              <inertial pos="0.0145671 5e-05 0.016497" quat="0.977773 0 0.209665 0" mass="0.0426859" diaginertia="1.20653e-05 1.19789e-05 2.64405e-06"/>
-                              <joint name="robotiq_85_right_finger_tip_joint" pos="0 0 0" axis="0 -1 0"/>
-                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="right_finger_tip" name="right_finger_tip"/>
-                              <geom type="mesh" mesh="right_finger_tip" name="right_finger_tip2"/>
+                        <body
+                          name="robotiq_85_right_knuckle_link"
+                          pos="-0.0306011 0 0.0549045"
+                          gravcomp="1"
+                        >
+                          <inertial
+                            pos="-0.012132 0.0002 -0.00058647"
+                            quat="0.5153 0.484217 0.484217 0.5153"
+                            mass="0.0138477"
+                            diaginertia="2.31944e-06 2.23867e-06 3.45008e-07"
+                          />
+                          <joint
+                            name="robotiq_85_right_knuckle_joint"
+                            pos="0 0 0"
+                            axis="0 -1 0"
+                            range="-0.8 0"
+                            actuatorfrcrange="-50 50"
+                          />
+                          <geom
+                            type="mesh"
+                            contype="0"
+                            conaffinity="0"
+                            group="1"
+                            density="0"
+                            mesh="right_knuckle"
+                            name="right_knuckle"
+                          />
+                          <geom
+                            type="mesh"
+                            mesh="right_knuckle"
+                            name="right_knuckle2"
+                          />
+                          <body
+                            name="robotiq_85_right_finger_link"
+                            pos="-0.0315262 0 -0.00376347"
+                            gravcomp="1"
+                          >
+                            <inertial
+                              pos="-0.00346899 -5.53e-06 0.0186712"
+                              quat="0.99325 0 -0.115989 0"
+                              mass="0.0426038"
+                              diaginertia="1.43724e-05 1.18321e-05 4.68228e-06"
+                            />
+                            <geom
+                              type="mesh"
+                              contype="0"
+                              conaffinity="0"
+                              group="1"
+                              density="0"
+                              mesh="right_finger"
+                              name="right_finger"
+                            />
+                            <geom
+                              type="mesh"
+                              mesh="right_finger"
+                              name="right_finger2"
+                            />
+                            <body
+                              name="robotiq_85_right_finger_tip_link"
+                              pos="-0.00563134 0 0.0471852"
+                              gravcomp="1"
+                            >
+                              <inertial
+                                pos="0.0145671 5e-05 0.016497"
+                                quat="0.977773 0 0.209665 0"
+                                mass="0.0426859"
+                                diaginertia="1.20653e-05 1.19789e-05 2.64405e-06"
+                              />
+                              <joint
+                                name="robotiq_85_right_finger_tip_joint"
+                                pos="0 0 0"
+                                axis="0 -1 0"
+                              />
+                              <geom
+                                type="mesh"
+                                contype="0"
+                                conaffinity="0"
+                                group="1"
+                                density="0"
+                                mesh="right_finger_tip"
+                                name="right_finger_tip"
+                              />
+                              <geom
+                                type="mesh"
+                                mesh="right_finger_tip"
+                                name="right_finger_tip2"
+                              />
                             </body>
                           </body>
                         </body>
-                        <body name="robotiq_85_left_inner_knuckle_link" pos="0.0127 0 0.06142" gravcomp="1">
-                          <inertial pos="0.018977 0.00015001 0.022471" quat="0.939613 0 0.342237 0" mass="0.0296938" diaginertia="1.28757e-05 8.69056e-06 4.88712e-06"/>
-                          <joint name="robotiq_85_left_inner_knuckle_joint" pos="0 0 0" axis="0 -1 0"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="left_inner_knuckle" name="left_inner_knuckle"/>
-                          <geom type="mesh" mesh="left_inner_knuckle" name="left_inner_knuckle2"/>
+                        <body
+                          name="robotiq_85_left_inner_knuckle_link"
+                          pos="0.0127 0 0.06142"
+                          gravcomp="1"
+                        >
+                          <inertial
+                            pos="0.018977 0.00015001 0.022471"
+                            quat="0.939613 0 0.342237 0"
+                            mass="0.0296938"
+                            diaginertia="1.28757e-05 8.69056e-06 4.88712e-06"
+                          />
+                          <joint
+                            name="robotiq_85_left_inner_knuckle_joint"
+                            pos="0 0 0"
+                            axis="0 -1 0"
+                          />
+                          <geom
+                            type="mesh"
+                            contype="0"
+                            conaffinity="0"
+                            group="1"
+                            density="0"
+                            mesh="left_inner_knuckle"
+                            name="left_inner_knuckle"
+                          />
+                          <geom
+                            type="mesh"
+                            mesh="left_inner_knuckle"
+                            name="left_inner_knuckle2"
+                          />
                         </body>
-                        <body name="robotiq_85_right_inner_knuckle_link" pos="-0.0127 0 0.06142" gravcomp="1">
-                          <inertial pos="-0.0192682 5.001e-05 0.0222218" quat="0.936371 0 -0.351012 0" mass="0.0296938" diaginertia="1.2882e-05 8.69056e-06 4.88076e-06"/>
-                          <joint name="robotiq_85_right_inner_knuckle_joint" pos="0 0 0" axis="0 -1 0"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="right_inner_knuckle" name="right_inner_knuckle"/>
-                          <geom type="mesh" mesh="right_inner_knuckle" name="right_inner_knuckle2"/>
+                        <body
+                          name="robotiq_85_right_inner_knuckle_link"
+                          pos="-0.0127 0 0.06142"
+                          gravcomp="1"
+                        >
+                          <inertial
+                            pos="-0.0192682 5.001e-05 0.0222218"
+                            quat="0.936371 0 -0.351012 0"
+                            mass="0.0296938"
+                            diaginertia="1.2882e-05 8.69056e-06 4.88076e-06"
+                          />
+                          <joint
+                            name="robotiq_85_right_inner_knuckle_joint"
+                            pos="0 0 0"
+                            axis="0 -1 0"
+                          />
+                          <geom
+                            type="mesh"
+                            contype="0"
+                            conaffinity="0"
+                            group="1"
+                            density="0"
+                            mesh="right_inner_knuckle"
+                            name="right_inner_knuckle"
+                          />
+                          <geom
+                            type="mesh"
+                            mesh="right_inner_knuckle"
+                            name="right_inner_knuckle2"
+                          />
                         </body>
-                        <body name="grasp_link" gravcomp="1"/>
+                        <body name="grasp_link" gravcomp="1" />
                       </body>
                     </body>
                   </body>
@@ -129,4 +545,4 @@
       </body>
     </body>
   </worldbody>
-</mujoco>
\ No newline at end of file
+</mujoco>
diff --git a/src/example_robot_sim/mjcf/assets/scene.xml b/src/example_robot_sim/mjcf/assets/scene.xml
index 5694b27..2bb5a60 100644
--- a/src/example_robot_sim/mjcf/assets/scene.xml
+++ b/src/example_robot_sim/mjcf/assets/scene.xml
@@ -1,40 +1,68 @@
 <mujoco model="2f85 scene">
-  <include file="2f85.xml"/>
+  <include file="2f85.xml" />
 
   <!-- Add some fluid viscosity to prevent the hanging box from jiggling forever -->
-  <option viscosity="0.1"/>
+  <option viscosity="0.1" />
 
-  <statistic center="0 0 0.05" extent="0.3"/>
+  <statistic center="0 0 0.05" extent="0.3" />
 
   <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="60" 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="60" 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="skybox"
+      builtin="gradient"
+      rgb1="0.3 0.5 0.7"
+      rgb2="0 0 0"
+      width="512"
+      height="3072"
+    />
+    <texture
+      type="2d"
+      name="groundplane"
+      builtin="checker"
+      mark="edge"
+      rgb1="0.2 0.3 0.4"
+      rgb2="0.1 0.2 0.3"
+      markrgb="0.8 0.8 0.8"
+      width="300"
+      height="300"
+    />
+    <material
+      name="groundplane"
+      texture="groundplane"
+      texuniform="true"
+      texrepeat="5 5"
+      reflectance="0.2"
+    />
   </asset>
 
   <worldbody>
-    <light pos="0 0 1"/>
-    <light pos="0 -0.2 1" dir="0 0.2 -0.8" directional="true"/>
-    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
-    <site size="0.002" pos="0 0 0.2" name="anchor"/>
+    <light pos="0 0 1" />
+    <light pos="0 -0.2 1" dir="0 0.2 -0.8" directional="true" />
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
+    <site size="0.002" pos="0 0 0.2" name="anchor" />
     <body name="object" pos="0 0 0.15">
-      <freejoint/>
-      <geom type="box" size="0.015 0.015 0.015" rgba=".5 .7 .5 1" friction=".5" priority="1"/>
-      <site size="0.002" pos="0.015 0.015 0.015" name="hook"/>
+      <freejoint />
+      <geom
+        type="box"
+        size="0.015 0.015 0.015"
+        rgba=".5 .7 .5 1"
+        friction=".5"
+        priority="1"
+      />
+      <site size="0.002" pos="0.015 0.015 0.015" name="hook" />
     </body>
   </worldbody>
 
   <tendon>
     <spatial limited="true" range="0 0.02" width="0.001">
-      <site site="hook"/>
-      <site site="anchor"/>
+      <site site="hook" />
+      <site site="anchor" />
     </spatial>
   </tendon>
 </mujoco>
diff --git a/src/example_robot_sim/package.xml b/src/example_robot_sim/package.xml
index 071bd37..e7a0823 100644
--- a/src/example_robot_sim/package.xml
+++ b/src/example_robot_sim/package.xml
@@ -4,10 +4,9 @@
   <version>0.0.1</version>
   <description>My robot configuration package</description>
 
-  <maintainer email="support@picknik.ai">
-    MoveIt Pro User
-  </maintainer>
-  <license>Apache-2.0</license>
+  <maintainer email="support@picknik.ai">MoveIt Pro User</maintainer>
+  <license>BSD-3-Clause</license>
+  <license file="mjcf/assets/LICENSE">BSD-2-Clause</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>example_robot_mock</depend>
diff --git a/src/example_robot_sim/urdf/r6bot_mujoco_ros2_control_macro.xacro b/src/example_robot_sim/urdf/r6bot_mujoco_ros2_control_macro.xacro
index 35e0605..4b11124 100644
--- a/src/example_robot_sim/urdf/r6bot_mujoco_ros2_control_macro.xacro
+++ b/src/example_robot_sim/urdf/r6bot_mujoco_ros2_control_macro.xacro
@@ -25,4 +25,4 @@
       </hardware>
     </ros2_control>
   </xacro:macro>
-</robot>
\ No newline at end of file
+</robot>