Mm sync (#18)

Sync changes from moveit_pro_mobile_maipulation

src/moveit_pro_mobile_manipulation/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>

src/moveit_pro_mobile_manipulation/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">

src/moveit_pro_mobile_manipulation/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>

src/moveit_pro_mobile_manipulation/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>