diff --git a/README.md b/README.md
index 04f9294..1c2fcf7 100644
--- a/README.md
+++ b/README.md
@@ -1 +1 @@
-# moveit_pro_mobile_manipulation
\ No newline at end of file
+# moveit_pro_mobile_manipulation
diff --git a/mobile_manipulation_config/description/scene.xml b/mobile_manipulation_config/description/scene.xml
index 7e766c3..68621d6 100644
--- a/mobile_manipulation_config/description/scene.xml
+++ b/mobile_manipulation_config/description/scene.xml
@@ -39,4 +39,22 @@
     </body>
     <include file="ridgeback/ridgeback.xml" />
   </worldbody>
+  <keyframe>
+    <!-- qpos -->
+    <!-- front_rocker, front_left_wheel, front_right_wheel, rear_left_wheel, rear_right_wheel -->
+    <!-- linear_x_joint, linear_y_joint, rotational_yaw_joint, shoulder_pan, shoulder_lift, elbow, wrist_1, wrist_2, wrist_3 -->
+    <!-- right_knuckle, right_finger, right_inner_knuckle, right_finger_tip, left_knuckle, left_finger, left_inner_knuckle, left_finger_tip -->
+    <!-- ctrl -->
+    <!-- linear_x_joint, linear_y_joint, rotational_yaw_joint, shoulder_pan, shoulder_lift, elbow, wrist_1, wrist_2, wrist_3, robotiq_85_left_knuckle_joint -->
+    <key
+      qpos="
+      0 0 0 0 0
+      0 0 0 0 -1.534 0.8197 -2.2686 -1.57076 0
+      0 0 0 0 0 0 0 0
+    "
+      ctrl="
+      0 0 0 0 -1.534 0.8197 -2.2686 -1.57076 0 0
+    "
+    />
+  </keyframe>
 </mujoco>
diff --git a/mobile_manipulation_config/package.xml b/mobile_manipulation_config/package.xml
index 53a1f7e..7b94036 100644
--- a/mobile_manipulation_config/package.xml
+++ b/mobile_manipulation_config/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0" ?>
 <package format="3">
   <name>mobile_manipulation_config</name>
-  <version>6.1.1</version>
+  <version>6.5.0</version>
 
   <description>
     MuJoCo simulation configuration package for scan and spray application
diff --git a/picknik_ur_mobile_config/config/moveit/picknik_ur.srdf b/picknik_ur_mobile_config/config/moveit/picknik_ur.srdf
index e0be97c..a3a8507 100644
--- a/picknik_ur_mobile_config/config/moveit/picknik_ur.srdf
+++ b/picknik_ur_mobile_config/config/moveit/picknik_ur.srdf
@@ -19,17 +19,9 @@
         <group name="linear_actuator" />
     </group>
     <group name="gripper">
-        <link name="robotiq_85_base_link"/>
-        <link name="robotiq_85_left_inner_knuckle_link"/>
-        <link name="robotiq_85_left_finger_tip_link"/>
-        <link name="robotiq_85_left_knuckle_link"/>
-        <link name="robotiq_85_left_finger_link"/>
-        <link name="robotiq_85_right_inner_knuckle_link"/>
-        <link name="robotiq_85_right_finger_tip_link"/>
-        <link name="robotiq_85_right_knuckle_link"/>
-        <link name="robotiq_85_right_finger_link"/>
-        <link name="grasp_link"/>
-        <joint name="robotiq_85_left_knuckle_joint"/>
+        <chain base_link="robotiq_85_base_link" tip_link="robotiq_85_left_finger_link"/>
+        <chain base_link="robotiq_85_base_link" tip_link="robotiq_85_right_finger_link"/>
+        <chain base_link="robotiq_85_base_link" tip_link="grasp_link"/>
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="open" group="gripper">
diff --git a/picknik_ur_mobile_config/description/scene.xml b/picknik_ur_mobile_config/description/scene.xml
index c7e3b8b..d580271 100644
--- a/picknik_ur_mobile_config/description/scene.xml
+++ b/picknik_ur_mobile_config/description/scene.xml
@@ -6,8 +6,6 @@
   <include file="ridgeback/ridgeback_globals.xml" />
 
   <compiler angle="radian" autolimits="true" />
-
-  <option integrator="implicitfast" />
   <statistic center="0.3 0 0.4" extent="0.8" />
 
   <worldbody>
diff --git a/picknik_ur_mobile_config/objectives/apriltag_pick_object.xml b/picknik_ur_mobile_config/objectives/apriltag_pick_object.xml
index 2f4e297..ad22362 100644
--- a/picknik_ur_mobile_config/objectives/apriltag_pick_object.xml
+++ b/picknik_ur_mobile_config/objectives/apriltag_pick_object.xml
@@ -7,7 +7,7 @@
     _favorite="true"
   >
     <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree ID="Wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
       <Control ID="Sequence" name="Setup">
         <SubTree ID="Open Gripper" />
       </Control>
diff --git a/picknik_ur_mobile_config/objectives/pick_up_cube.xml b/picknik_ur_mobile_config/objectives/pick_up_cube.xml
index 8bb2bea..670e2cb 100644
--- a/picknik_ur_mobile_config/objectives/pick_up_cube.xml
+++ b/picknik_ur_mobile_config/objectives/pick_up_cube.xml
@@ -4,7 +4,7 @@
   <BehaviorTree ID="Pick up Cube" _description="" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
-      <SubTree ID="Wrist camera snapshot" _collapsed="true" />
+      <SubTree ID="Take wrist camera snapshot" _collapsed="true" />
       <SubTree ID="Pick object" _collapsed="false" />
     </Control>
   </BehaviorTree>
diff --git a/picknik_ur_mobile_config/objectives/take_snap.xml b/picknik_ur_mobile_config/objectives/take_snap.xml
index 7fefe4d..07878d8 100644
--- a/picknik_ur_mobile_config/objectives/take_snap.xml
+++ b/picknik_ur_mobile_config/objectives/take_snap.xml
@@ -1,7 +1,11 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Scene camera snapshot">
+<root BTCPP_format="4" main_tree_to_execute="Take scene camera snapshot">
   <!--//////////-->
-  <BehaviorTree ID="Scene camera snapshot" _description="" _favorite="true">
+  <BehaviorTree
+    ID="Take scene camera snapshot"
+    _description=""
+    _favorite="true"
+  >
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Clear out old snapshot data -->
       <Action ID="ClearSnapshot" />
@@ -10,6 +14,6 @@
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Scene camera snapshot" />
+    <SubTree ID="Take scene camera snapshot" />
   </TreeNodesModel>
 </root>
diff --git a/picknik_ur_mobile_config/objectives/wrist_snap.xml b/picknik_ur_mobile_config/objectives/wrist_snap.xml
index 80209f3..97c4f73 100644
--- a/picknik_ur_mobile_config/objectives/wrist_snap.xml
+++ b/picknik_ur_mobile_config/objectives/wrist_snap.xml
@@ -1,7 +1,11 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Wrist camera snapshot">
+<root BTCPP_format="4" main_tree_to_execute="Take wrist camera snapshot">
   <!--//////////-->
-  <BehaviorTree ID="Wrist camera snapshot" _description="" _favorite="true">
+  <BehaviorTree
+    ID="Take wrist camera snapshot"
+    _description=""
+    _favorite="true"
+  >
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Do not clear snapshots first, so we can take multiple snapshots from different angles -->
       <Action ID="GetPointCloud" topic_name="/wrist_camera/points" />
@@ -9,6 +13,6 @@
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Wrist camera snapshot" />
+    <SubTree ID="Take wrist camera snapshot" />
   </TreeNodesModel>
 </root>
diff --git a/picknik_ur_mobile_config/package.xml b/picknik_ur_mobile_config/package.xml
index 70f401a..8121bd0 100644
--- a/picknik_ur_mobile_config/package.xml
+++ b/picknik_ur_mobile_config/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0" ?>
 <package format="3">
   <name>picknik_ur_mobile_config</name>
-  <version>6.0.0</version>
+  <version>6.5.0</version>
 
   <description>
     MuJoCo simulation configuration package for Picknik's UR robot on a mobile