Cleaned URDF and MoveIt! config

nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H21.srdf

@@ -43,9 +43,9 @@
         <joint name="RAnklePitch" />
         <joint name="RAnkleRoll" />
     </group>
-    <group name="Pelvis">
+    <!-- <group name="Pelvis">
         <joint name="LHipYawPitch" />
-    </group>
+    </group> -->
     <group name="Body">
         <joint name="HeadYaw" />
         <joint name="HeadPitch" />
@@ -53,10 +53,12 @@
         <joint name="LShoulderRoll" />
         <joint name="LElbowYaw" />
         <joint name="LElbowRoll" />
+        <joint name="LWristYaw" />
         <joint name="RShoulderPitch" />
         <joint name="RShoulderRoll" />
         <joint name="RElbowYaw" />
         <joint name="RElbowRoll" />
+        <joint name="RWristYaw" />
         <joint name="LHipYawPitch" />
         <joint name="LHipRoll" />
         <joint name="LHipPitch" />
@@ -78,7 +80,6 @@
     <passive_joint name="virtual_joint" />
     <passive_joint name="RHipYawPitch" />
     <passive_joint name="LHand" />
-    <passive_joint name="RHand" />
     <passive_joint name="LFinger1" />
     <passive_joint name="LFinger1_1" />
     <passive_joint name="LFinger1_2" />
@@ -87,6 +88,7 @@
     <passive_joint name="LFinger2_2" />
     <passive_joint name="LThumb1_joint" />
     <passive_joint name="LThumb1_2" />
+    <passive_joint name="RHand" />
     <passive_joint name="RFinger1" />
     <passive_joint name="RFinger1_1" />
     <passive_joint name="RFinger1_2" />
@@ -107,6 +109,8 @@
     <disable_collisions link1="LElbowDummy" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="LThumb2" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="RElbowDummy" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="RFinger11" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="RFinger21" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="RHip" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="RHipDummy" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="RHipYaw" reason="Never" />
@@ -116,7 +120,6 @@
     <disable_collisions link1="LElbowDummy" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="l_wrist" reason="Adjacent" />
     <disable_collisions link1="LElbowDummy" link2="neck" reason="Never" />
-    <disable_collisions link1="LElbowDummy" link2="odroid" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="r_ankle" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="r_sole" reason="Never" />
     <disable_collisions link1="LFinger11" link2="LFinger12" reason="Adjacent" />
@@ -128,12 +131,13 @@
     <disable_collisions link1="LFinger11" link2="LShoulderDummy" reason="Never" />
     <disable_collisions link1="LFinger11" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LFinger11" link2="LThumb2" reason="Default" />
-    <disable_collisions link1="LFinger11" link2="RElbowDummy" reason="Never" />
-    <disable_collisions link1="LFinger11" link2="RFinger13" reason="Never" />
     <disable_collisions link1="LFinger11" link2="RKnee" reason="Never" />
     <disable_collisions link1="LFinger11" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LFinger11" link2="l_wrist" reason="Adjacent" />
     <disable_collisions link1="LFinger11" link2="neck" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="r_ankle" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="r_sole" reason="Never" />
     <disable_collisions link1="LFinger12" link2="LFinger13" reason="Adjacent" />
     <disable_collisions link1="LFinger12" link2="LFinger21" reason="Never" />
     <disable_collisions link1="LFinger12" link2="LFinger22" reason="Never" />
@@ -142,11 +146,11 @@
     <disable_collisions link1="LFinger12" link2="LShoulderDummy" reason="Never" />
     <disable_collisions link1="LFinger12" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LFinger12" link2="LThumb2" reason="Default" />
-    <disable_collisions link1="LFinger12" link2="RFinger13" reason="Never" />
+    <disable_collisions link1="LFinger12" link2="RKnee" reason="Never" />
     <disable_collisions link1="LFinger12" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LFinger12" link2="l_wrist" reason="Default" />
     <disable_collisions link1="LFinger12" link2="neck" reason="Never" />
-    <disable_collisions link1="LFinger12" link2="odroid" reason="Never" />
+    <disable_collisions link1="LFinger12" link2="r_sole" reason="Never" />
     <disable_collisions link1="LFinger13" link2="LFinger21" reason="Never" />
     <disable_collisions link1="LFinger13" link2="LFinger22" reason="Never" />
     <disable_collisions link1="LFinger13" link2="LFinger23" reason="Never" />
@@ -157,12 +161,14 @@
     <disable_collisions link1="LFinger13" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LFinger13" link2="l_wrist" reason="Default" />
     <disable_collisions link1="LFinger13" link2="neck" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="r_ankle" reason="Never" />
     <disable_collisions link1="LFinger21" link2="LFinger22" reason="Adjacent" />
     <disable_collisions link1="LFinger21" link2="LFinger23" reason="Never" />
     <disable_collisions link1="LFinger21" link2="LShoulder" reason="Never" />
     <disable_collisions link1="LFinger21" link2="LShoulderDummy" reason="Never" />
     <disable_collisions link1="LFinger21" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LFinger21" link2="LThumb2" reason="Never" />
+    <disable_collisions link1="LFinger21" link2="RFinger21" reason="Never" />
     <disable_collisions link1="LFinger21" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LFinger21" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LFinger21" link2="l_wrist" reason="Adjacent" />
@@ -180,8 +186,8 @@
     <disable_collisions link1="LFinger23" link2="LShoulderDummy" reason="Never" />
     <disable_collisions link1="LFinger23" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LFinger23" link2="LThumb2" reason="Never" />
-    <disable_collisions link1="LFinger23" link2="RFinger11" reason="Never" />
     <disable_collisions link1="LFinger23" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LFinger23" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LFinger23" link2="l_wrist" reason="Never" />
     <disable_collisions link1="LFinger23" link2="neck" reason="Never" />
     <disable_collisions link1="LHip" link2="LHipDummy" reason="Adjacent" />
@@ -196,13 +202,12 @@
     <disable_collisions link1="LHip" link2="RKnee" reason="Never" />
     <disable_collisions link1="LHip" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LHip" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="LHip" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LHip" link2="gaze" reason="Never" />
-    <disable_collisions link1="LHip" link2="helmet" reason="Never" />
     <disable_collisions link1="LHip" link2="l_ankle" reason="Never" />
     <disable_collisions link1="LHip" link2="neck" reason="Never" />
-    <disable_collisions link1="LHip" link2="odroid" reason="Never" />
-    <disable_collisions link1="LHip" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="LHip" link2="r_sole" reason="Never" />
+    <disable_collisions link1="LHip" link2="torso" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="LHipYaw" reason="Default" />
     <disable_collisions link1="LHipDummy" link2="LKnee" reason="Adjacent" />
     <disable_collisions link1="LHipDummy" link2="LShoulder" reason="Never" />
@@ -212,17 +217,13 @@
     <disable_collisions link1="LHipDummy" link2="RHipYaw" reason="Default" />
     <disable_collisions link1="LHipDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="gaze" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="helmet" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="l_ankle" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="neck" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="odroid" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="torso" reason="Default" />
     <disable_collisions link1="LHipYaw" link2="LKnee" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="LShoulder" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="LShoulderDummy" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="RElbowDummy" reason="Never" />
-    <disable_collisions link1="LHipYaw" link2="RFinger21" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="RHip" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="RHipDummy" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="RHipYaw" reason="Adjacent" />
@@ -230,25 +231,19 @@
     <disable_collisions link1="LHipYaw" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="RThumb1" reason="Never" />
+    <disable_collisions link1="LHipYaw" link2="RThumb2" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="gaze" reason="Never" />
-    <disable_collisions link1="LHipYaw" link2="helmet" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="l_ankle" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="neck" reason="Never" />
-    <disable_collisions link1="LHipYaw" link2="odroid" reason="Never" />
-    <disable_collisions link1="LHipYaw" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="torso" reason="Adjacent" />
     <disable_collisions link1="LKnee" link2="LShoulder" reason="Never" />
-    <disable_collisions link1="LKnee" link2="RElbowDummy" reason="Never" />
     <disable_collisions link1="LKnee" link2="RKnee" reason="Default" />
     <disable_collisions link1="LKnee" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LKnee" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LKnee" link2="gaze" reason="Never" />
-    <disable_collisions link1="LKnee" link2="helmet" reason="Never" />
     <disable_collisions link1="LKnee" link2="l_ankle" reason="Adjacent" />
+    <disable_collisions link1="LKnee" link2="l_sole" reason="Never" />
     <disable_collisions link1="LKnee" link2="neck" reason="Never" />
-    <disable_collisions link1="LKnee" link2="odroid" reason="Never" />
-    <disable_collisions link1="LKnee" link2="odroid_bag" reason="Never" />
-    <disable_collisions link1="LKnee" link2="l_sole" reason="Adjacent" />
     <disable_collisions link1="LShoulder" link2="LShoulderDummy" reason="Adjacent" />
     <disable_collisions link1="LShoulder" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LShoulder" link2="LThumb2" reason="Never" />
@@ -268,13 +263,10 @@
     <disable_collisions link1="LShoulder" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LShoulder" link2="RThumb2" reason="Never" />
     <disable_collisions link1="LShoulder" link2="gaze" reason="Default" />
-    <disable_collisions link1="LShoulder" link2="helmet" reason="Never" />
     <disable_collisions link1="LShoulder" link2="l_ankle" reason="Never" />
     <disable_collisions link1="LShoulder" link2="l_sole" reason="Never" />
     <disable_collisions link1="LShoulder" link2="l_wrist" reason="Never" />
     <disable_collisions link1="LShoulder" link2="neck" reason="Never" />
-    <disable_collisions link1="LShoulder" link2="odroid" reason="Never" />
-    <disable_collisions link1="LShoulder" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="LShoulder" link2="r_ankle" reason="Never" />
     <disable_collisions link1="LShoulder" link2="r_sole" reason="Never" />
     <disable_collisions link1="LShoulder" link2="r_wrist" reason="Never" />
@@ -282,6 +274,7 @@
     <disable_collisions link1="LShoulderDummy" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="LThumb2" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RElbowDummy" reason="Never" />
+    <disable_collisions link1="LShoulderDummy" link2="RFinger21" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RHip" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RHipDummy" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RHipYaw" reason="Never" />
@@ -291,27 +284,30 @@
     <disable_collisions link1="LShoulderDummy" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RThumb2" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="gaze" reason="Default" />
+    <disable_collisions link1="LShoulderDummy" link2="l_ankle" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="l_wrist" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="neck" reason="Never" />
-    <disable_collisions link1="LShoulderDummy" link2="odroid" reason="Never" />
-    <disable_collisions link1="LShoulderDummy" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="r_ankle" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="r_sole" reason="Never" />
-    <disable_collisions link1="LShoulderDummy" link2="r_wrist" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="torso" reason="Default" />
     <disable_collisions link1="LThumb1" link2="LThumb2" reason="Adjacent" />
+    <disable_collisions link1="LThumb1" link2="RElbowDummy" reason="Never" />
     <disable_collisions link1="LThumb1" link2="RHip" reason="Never" />
     <disable_collisions link1="LThumb1" link2="RHipDummy" reason="Never" />
     <disable_collisions link1="LThumb1" link2="RKnee" reason="Never" />
     <disable_collisions link1="LThumb1" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LThumb1" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="LThumb1" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LThumb1" link2="l_wrist" reason="Adjacent" />
     <disable_collisions link1="LThumb1" link2="neck" reason="Never" />
     <disable_collisions link1="LThumb1" link2="r_ankle" reason="Never" />
     <disable_collisions link1="LThumb1" link2="r_sole" reason="Never" />
+    <disable_collisions link1="LThumb2" link2="RElbowDummy" reason="Never" />
+    <disable_collisions link1="LThumb2" link2="RHip" reason="Never" />
     <disable_collisions link1="LThumb2" link2="RKnee" reason="Never" />
     <disable_collisions link1="LThumb2" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LThumb2" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="LThumb2" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LThumb2" link2="l_wrist" reason="Default" />
     <disable_collisions link1="LThumb2" link2="neck" reason="Never" />
     <disable_collisions link1="LThumb2" link2="r_ankle" reason="Never" />
@@ -324,12 +320,12 @@
     <disable_collisions link1="RElbowDummy" link2="RFinger23" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="RShoulderDummy" reason="Adjacent" />
+    <disable_collisions link1="RElbowDummy" link2="RThumb1" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="RThumb2" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="l_sole" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="neck" reason="Never" />
-    <disable_collisions link1="RElbowDummy" link2="odroid" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="r_wrist" reason="Adjacent" />
-    <disable_collisions link1="RElbowDummy" link2="torso" reason="Never" />
     <disable_collisions link1="RFinger11" link2="RFinger12" reason="Adjacent" />
     <disable_collisions link1="RFinger11" link2="RFinger13" reason="Never" />
     <disable_collisions link1="RFinger11" link2="RFinger21" reason="Never" />
@@ -339,12 +335,9 @@
     <disable_collisions link1="RFinger11" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RFinger11" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RFinger11" link2="RThumb2" reason="Default" />
-    <disable_collisions link1="RFinger11" link2="LElbowDummy" reason="Never" />
+    <disable_collisions link1="RFinger11" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RFinger11" link2="LFinger13" reason="Never" />
-    <disable_collisions link1="RFinger11" link2="LKnee" reason="Never" />
-    <disable_collisions link1="RFinger11" link2="LShoulder" reason="Never" />
-    <disable_collisions link1="RFinger11" link2="r_wrist" reason="Adjacent" />
     <disable_collisions link1="RFinger11" link2="neck" reason="Never" />
+    <disable_collisions link1="RFinger11" link2="r_wrist" reason="Adjacent" />
     <disable_collisions link1="RFinger12" link2="RFinger13" reason="Adjacent" />
     <disable_collisions link1="RFinger12" link2="RFinger21" reason="Never" />
     <disable_collisions link1="RFinger12" link2="RFinger22" reason="Never" />
@@ -353,11 +346,9 @@
     <disable_collisions link1="RFinger12" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RFinger12" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RFinger12" link2="RThumb2" reason="Default" />
-    <disable_collisions link1="RFinger12" link2="LFinger13" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RFinger12" link2="LShoulder" reason="Never" />
-    <disable_collisions link1="RFinger12" link2="r_wrist" reason="Default" />
     <disable_collisions link1="RFinger12" link2="neck" reason="Never" />
-    <disable_collisions link1="RFinger12" link2="odroid" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="r_wrist" reason="Default" />
     <disable_collisions link1="RFinger13" link2="RFinger21" reason="Never" />
     <disable_collisions link1="RFinger13" link2="RFinger22" reason="Never" />
     <disable_collisions link1="RFinger13" link2="RFinger23" reason="Never" />
@@ -365,90 +356,70 @@
     <disable_collisions link1="RFinger13" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RFinger13" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RFinger13" link2="RThumb2" reason="Default" />
-    <disable_collisions link1="RFinger13" link2="LShoulder" reason="Never" />
+    <disable_collisions link1="RFinger13" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RFinger13" link2="r_wrist" reason="Default" />
     <disable_collisions link1="RFinger13" link2="neck" reason="Never" />
+    <disable_collisions link1="RFinger13" link2="r_wrist" reason="Default" />
     <disable_collisions link1="RFinger21" link2="RFinger22" reason="Adjacent" />
     <disable_collisions link1="RFinger21" link2="RFinger23" reason="Never" />
     <disable_collisions link1="RFinger21" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RFinger21" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RFinger21" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RFinger21" link2="RThumb2" reason="Never" />
-    <disable_collisions link1="RFinger21" link2="LShoulder" reason="Never" />
+    <disable_collisions link1="RFinger21" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RFinger21" link2="LShoulderDummy" reason="Never" />
-    <disable_collisions link1="RFinger21" link2="r_wrist" reason="Adjacent" />
     <disable_collisions link1="RFinger21" link2="neck" reason="Never" />
-    <disable_collisions link1="RFinger21" link2="l_sole" reason="Never" />
+    <disable_collisions link1="RFinger21" link2="r_wrist" reason="Adjacent" />
     <disable_collisions link1="RFinger22" link2="RFinger23" reason="Adjacent" />
     <disable_collisions link1="RFinger22" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RFinger22" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RFinger22" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RFinger22" link2="RThumb2" reason="Never" />
-    <disable_collisions link1="RFinger22" link2="LShoulder" reason="Never" />
+    <disable_collisions link1="RFinger22" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RFinger22" link2="r_wrist" reason="Never" />
     <disable_collisions link1="RFinger22" link2="neck" reason="Never" />
+    <disable_collisions link1="RFinger22" link2="r_wrist" reason="Never" />
     <disable_collisions link1="RFinger23" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RFinger23" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RFinger23" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RFinger23" link2="RThumb2" reason="Never" />
-    <disable_collisions link1="RFinger23" link2="LFinger11" reason="Never" />
+    <disable_collisions link1="RFinger23" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RFinger23" link2="LShoulder" reason="Never" />
-    <disable_collisions link1="RFinger23" link2="r_wrist" reason="Never" />
     <disable_collisions link1="RFinger23" link2="neck" reason="Never" />
+    <disable_collisions link1="RFinger23" link2="r_wrist" reason="Never" />
     <disable_collisions link1="RHip" link2="RHipDummy" reason="Adjacent" />
     <disable_collisions link1="RHip" link2="RHipYaw" reason="Adjacent" />
     <disable_collisions link1="RHip" link2="RKnee" reason="Never" />
     <disable_collisions link1="RHip" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RHip" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RHip" link2="gaze" reason="Never" />
-    <disable_collisions link1="RHip" link2="helmet" reason="Never" />
     <disable_collisions link1="RHip" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RHip" link2="neck" reason="Never" />
-    <disable_collisions link1="RHip" link2="odroid" reason="Never" />
-    <disable_collisions link1="RHip" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="RHip" link2="r_ankle" reason="Never" />
     <disable_collisions link1="RHip" link2="torso" reason="Never" />
     <disable_collisions link1="RHipDummy" link2="RHipYaw" reason="Default" />
     <disable_collisions link1="RHipDummy" link2="RKnee" reason="Adjacent" />
     <disable_collisions link1="RHipDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RHipDummy" link2="gaze" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="helmet" reason="Never" />
     <disable_collisions link1="RHipDummy" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RHipDummy" link2="neck" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="odroid" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="RHipDummy" link2="r_ankle" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="torso" reason="Default" />
     <disable_collisions link1="RHipYaw" link2="RKnee" reason="Never" />
     <disable_collisions link1="RHipYaw" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RHipYaw" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RHipYaw" link2="gaze" reason="Never" />
-    <disable_collisions link1="RHipYaw" link2="helmet" reason="Never" />
-    <disable_collisions link1="RHipYaw" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RHipYaw" link2="neck" reason="Never" />
-    <disable_collisions link1="RHipYaw" link2="odroid" reason="Never" />
-    <disable_collisions link1="RHipYaw" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="RHipYaw" link2="r_ankle" reason="Never" />
     <disable_collisions link1="RHipYaw" link2="torso" reason="Adjacent" />
     <disable_collisions link1="RKnee" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RKnee" link2="gaze" reason="Never" />
-    <disable_collisions link1="RKnee" link2="helmet" reason="Never" />
     <disable_collisions link1="RKnee" link2="l_wrist" reason="Never" />
     <disable_collisions link1="RKnee" link2="neck" reason="Never" />
-    <disable_collisions link1="RKnee" link2="odroid" reason="Never" />
-    <disable_collisions link1="RKnee" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="RKnee" link2="r_ankle" reason="Adjacent" />
-    <disable_collisions link1="RKnee" link2="r_sole" reason="Adjacent" />
+    <disable_collisions link1="RKnee" link2="r_sole" reason="Never" />
     <disable_collisions link1="RShoulder" link2="RShoulderDummy" reason="Adjacent" />
     <disable_collisions link1="RShoulder" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RShoulder" link2="RThumb2" reason="Never" />
-    <disable_collisions link1="RShoulder" link2="helmet" reason="Never" />
     <disable_collisions link1="RShoulder" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RShoulder" link2="l_sole" reason="Never" />
     <disable_collisions link1="RShoulder" link2="l_wrist" reason="Never" />
     <disable_collisions link1="RShoulder" link2="neck" reason="Never" />
-    <disable_collisions link1="RShoulder" link2="odroid" reason="Never" />
-    <disable_collisions link1="RShoulder" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="RShoulder" link2="r_ankle" reason="Never" />
     <disable_collisions link1="RShoulder" link2="r_wrist" reason="Never" />
     <disable_collisions link1="RShoulder" link2="torso" reason="Adjacent" />
@@ -458,61 +429,32 @@
     <disable_collisions link1="RShoulderDummy" link2="l_sole" reason="Never" />
     <disable_collisions link1="RShoulderDummy" link2="l_wrist" reason="Never" />
     <disable_collisions link1="RShoulderDummy" link2="neck" reason="Never" />
-    <disable_collisions link1="RShoulderDummy" link2="odroid" reason="Never" />
     <disable_collisions link1="RShoulderDummy" link2="r_wrist" reason="Never" />
     <disable_collisions link1="RThumb1" link2="RThumb2" reason="Adjacent" />
     <disable_collisions link1="RThumb1" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RThumb1" link2="l_sole" reason="Never" />
     <disable_collisions link1="RThumb1" link2="neck" reason="Never" />
     <disable_collisions link1="RThumb1" link2="r_wrist" reason="Adjacent" />
     <disable_collisions link1="RThumb2" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RThumb2" link2="neck" reason="Never" />
     <disable_collisions link1="RThumb2" link2="r_wrist" reason="Default" />
-    <disable_collisions link1="gaze" link2="helmet" reason="Adjacent" />
     <disable_collisions link1="gaze" link2="l_ankle" reason="Never" />
     <disable_collisions link1="gaze" link2="l_sole" reason="Never" />
     <disable_collisions link1="gaze" link2="neck" reason="Adjacent" />
-    <disable_collisions link1="gaze" link2="odroid_bag" reason="Default" />
     <disable_collisions link1="gaze" link2="r_ankle" reason="Never" />
     <disable_collisions link1="gaze" link2="r_sole" reason="Never" />
     <disable_collisions link1="gaze" link2="torso" reason="Default" />
-    <disable_collisions link1="helmet" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="helmet" link2="l_sole" reason="Never" />
-    <disable_collisions link1="helmet" link2="neck" reason="Never" />
-    <disable_collisions link1="helmet" link2="odroid" reason="Never" />
-    <disable_collisions link1="helmet" link2="odroid_bag" reason="Never" />
-    <disable_collisions link1="helmet" link2="r_ankle" reason="Never" />
-    <disable_collisions link1="helmet" link2="r_sole" reason="Never" />
-    <disable_collisions link1="helmet" link2="torso" reason="Never" />
     <disable_collisions link1="l_ankle" link2="l_sole" reason="Adjacent" />
     <disable_collisions link1="l_ankle" link2="neck" reason="Never" />
-    <disable_collisions link1="l_ankle" link2="odroid" reason="Never" />
-    <disable_collisions link1="l_ankle" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="l_ankle" link2="r_wrist" reason="Never" />
-    <disable_collisions link1="l_ankle" link2="r_sole" reason="Never" />
     <disable_collisions link1="l_sole" link2="neck" reason="Never" />
-    <disable_collisions link1="l_sole" link2="odroid" reason="Never" />
-    <disable_collisions link1="l_sole" link2="odroid_bag" reason="Never" />
-    <disable_collisions link1="l_sole" link2="RKnee" reason="Adjacent" />
     <disable_collisions link1="l_wrist" link2="neck" reason="Never" />
     <disable_collisions link1="l_wrist" link2="r_ankle" reason="Never" />
     <disable_collisions link1="l_wrist" link2="r_sole" reason="Never" />
-    <disable_collisions link1="neck" link2="odroid" reason="Never" />
-    <disable_collisions link1="neck" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="neck" link2="r_ankle" reason="Never" />
     <disable_collisions link1="neck" link2="r_sole" reason="Never" />
     <disable_collisions link1="neck" link2="r_wrist" reason="Never" />
     <disable_collisions link1="neck" link2="torso" reason="Adjacent" />
-    <disable_collisions link1="odroid" link2="odroid_bag" reason="Adjacent" />
-    <disable_collisions link1="odroid" link2="r_ankle" reason="Never" />
-    <disable_collisions link1="odroid" link2="r_sole" reason="Never" />
-    <disable_collisions link1="odroid" link2="torso" reason="Never" />
-    <disable_collisions link1="odroid_bag" link2="r_ankle" reason="Never" />
-    <disable_collisions link1="odroid_bag" link2="r_sole" reason="Never" />
-    <disable_collisions link1="odroid_bag" link2="torso" reason="Adjacent" />
     <disable_collisions link1="r_ankle" link2="r_sole" reason="Adjacent" />
     <disable_collisions link1="r_ankle" link2="torso" reason="Never" />
-    <disable_collisions link1="r_ankle" link2="l_sole" reason="Never" />
     <disable_collisions link1="r_sole" link2="torso" reason="Never" />
-    <disable_collisions link1="r_sole" link2="LKnee" reason="Adjacent" />
 </robot>

nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H25.srdf

@@ -43,9 +43,9 @@
         <joint name="RAnklePitch" />
         <joint name="RAnkleRoll" />
     </group>
-    <group name="Pelvis">
+    <!-- <group name="Pelvis">
         <joint name="LHipYawPitch" />
-    </group>
+    </group> -->
     <group name="RightHand">
         <joint name="RWristYaw" />
         <joint name="RHand" />
@@ -135,6 +135,8 @@
     <disable_collisions link1="LElbowDummy" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="LThumb2" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="RElbowDummy" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="RFinger11" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="RFinger21" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="RHip" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="RHipDummy" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="RHipYaw" reason="Never" />
@@ -144,7 +146,6 @@
     <disable_collisions link1="LElbowDummy" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="l_wrist" reason="Adjacent" />
     <disable_collisions link1="LElbowDummy" link2="neck" reason="Never" />
-    <disable_collisions link1="LElbowDummy" link2="odroid" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="r_ankle" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="r_sole" reason="Never" />
     <disable_collisions link1="LFinger11" link2="LFinger12" reason="Adjacent" />
@@ -156,12 +157,13 @@
     <disable_collisions link1="LFinger11" link2="LShoulderDummy" reason="Never" />
     <disable_collisions link1="LFinger11" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LFinger11" link2="LThumb2" reason="Default" />
-    <disable_collisions link1="LFinger11" link2="RElbowDummy" reason="Never" />
-    <disable_collisions link1="LFinger11" link2="RFinger13" reason="Never" />
     <disable_collisions link1="LFinger11" link2="RKnee" reason="Never" />
     <disable_collisions link1="LFinger11" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LFinger11" link2="l_wrist" reason="Adjacent" />
     <disable_collisions link1="LFinger11" link2="neck" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="r_ankle" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="r_sole" reason="Never" />
     <disable_collisions link1="LFinger12" link2="LFinger13" reason="Adjacent" />
     <disable_collisions link1="LFinger12" link2="LFinger21" reason="Never" />
     <disable_collisions link1="LFinger12" link2="LFinger22" reason="Never" />
@@ -170,11 +172,11 @@
     <disable_collisions link1="LFinger12" link2="LShoulderDummy" reason="Never" />
     <disable_collisions link1="LFinger12" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LFinger12" link2="LThumb2" reason="Default" />
-    <disable_collisions link1="LFinger12" link2="RFinger13" reason="Never" />
+    <disable_collisions link1="LFinger12" link2="RKnee" reason="Never" />
     <disable_collisions link1="LFinger12" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LFinger12" link2="l_wrist" reason="Default" />
     <disable_collisions link1="LFinger12" link2="neck" reason="Never" />
-    <disable_collisions link1="LFinger12" link2="odroid" reason="Never" />
+    <disable_collisions link1="LFinger12" link2="r_sole" reason="Never" />
     <disable_collisions link1="LFinger13" link2="LFinger21" reason="Never" />
     <disable_collisions link1="LFinger13" link2="LFinger22" reason="Never" />
     <disable_collisions link1="LFinger13" link2="LFinger23" reason="Never" />
@@ -185,12 +187,14 @@
     <disable_collisions link1="LFinger13" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LFinger13" link2="l_wrist" reason="Default" />
     <disable_collisions link1="LFinger13" link2="neck" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="r_ankle" reason="Never" />
     <disable_collisions link1="LFinger21" link2="LFinger22" reason="Adjacent" />
     <disable_collisions link1="LFinger21" link2="LFinger23" reason="Never" />
     <disable_collisions link1="LFinger21" link2="LShoulder" reason="Never" />
     <disable_collisions link1="LFinger21" link2="LShoulderDummy" reason="Never" />
     <disable_collisions link1="LFinger21" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LFinger21" link2="LThumb2" reason="Never" />
+    <disable_collisions link1="LFinger21" link2="RFinger21" reason="Never" />
     <disable_collisions link1="LFinger21" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LFinger21" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LFinger21" link2="l_wrist" reason="Adjacent" />
@@ -208,8 +212,8 @@
     <disable_collisions link1="LFinger23" link2="LShoulderDummy" reason="Never" />
     <disable_collisions link1="LFinger23" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LFinger23" link2="LThumb2" reason="Never" />
-    <disable_collisions link1="LFinger23" link2="RFinger11" reason="Never" />
     <disable_collisions link1="LFinger23" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LFinger23" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LFinger23" link2="l_wrist" reason="Never" />
     <disable_collisions link1="LFinger23" link2="neck" reason="Never" />
     <disable_collisions link1="LHip" link2="LHipDummy" reason="Adjacent" />
@@ -224,13 +228,12 @@
     <disable_collisions link1="LHip" link2="RKnee" reason="Never" />
     <disable_collisions link1="LHip" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LHip" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="LHip" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LHip" link2="gaze" reason="Never" />
-    <disable_collisions link1="LHip" link2="helmet" reason="Never" />
     <disable_collisions link1="LHip" link2="l_ankle" reason="Never" />
     <disable_collisions link1="LHip" link2="neck" reason="Never" />
-    <disable_collisions link1="LHip" link2="odroid" reason="Never" />
-    <disable_collisions link1="LHip" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="LHip" link2="r_sole" reason="Never" />
+    <disable_collisions link1="LHip" link2="torso" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="LHipYaw" reason="Default" />
     <disable_collisions link1="LHipDummy" link2="LKnee" reason="Adjacent" />
     <disable_collisions link1="LHipDummy" link2="LShoulder" reason="Never" />
@@ -240,17 +243,13 @@
     <disable_collisions link1="LHipDummy" link2="RHipYaw" reason="Default" />
     <disable_collisions link1="LHipDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="gaze" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="helmet" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="l_ankle" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="neck" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="odroid" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="torso" reason="Default" />
     <disable_collisions link1="LHipYaw" link2="LKnee" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="LShoulder" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="LShoulderDummy" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="RElbowDummy" reason="Never" />
-    <disable_collisions link1="LHipYaw" link2="RFinger21" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="RHip" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="RHipDummy" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="RHipYaw" reason="Adjacent" />
@@ -258,25 +257,19 @@
     <disable_collisions link1="LHipYaw" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="RThumb1" reason="Never" />
+    <disable_collisions link1="LHipYaw" link2="RThumb2" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="gaze" reason="Never" />
-    <disable_collisions link1="LHipYaw" link2="helmet" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="l_ankle" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="neck" reason="Never" />
-    <disable_collisions link1="LHipYaw" link2="odroid" reason="Never" />
-    <disable_collisions link1="LHipYaw" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="torso" reason="Adjacent" />
     <disable_collisions link1="LKnee" link2="LShoulder" reason="Never" />
-    <disable_collisions link1="LKnee" link2="RElbowDummy" reason="Never" />
     <disable_collisions link1="LKnee" link2="RKnee" reason="Default" />
     <disable_collisions link1="LKnee" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LKnee" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LKnee" link2="gaze" reason="Never" />
-    <disable_collisions link1="LKnee" link2="helmet" reason="Never" />
     <disable_collisions link1="LKnee" link2="l_ankle" reason="Adjacent" />
+    <disable_collisions link1="LKnee" link2="l_sole" reason="Never" />
     <disable_collisions link1="LKnee" link2="neck" reason="Never" />
-    <disable_collisions link1="LKnee" link2="odroid" reason="Never" />
-    <disable_collisions link1="LKnee" link2="odroid_bag" reason="Never" />
-    <disable_collisions link1="LKnee" link2="l_sole" reason="Adjacent" />
     <disable_collisions link1="LShoulder" link2="LShoulderDummy" reason="Adjacent" />
     <disable_collisions link1="LShoulder" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LShoulder" link2="LThumb2" reason="Never" />
@@ -296,13 +289,10 @@
     <disable_collisions link1="LShoulder" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LShoulder" link2="RThumb2" reason="Never" />
     <disable_collisions link1="LShoulder" link2="gaze" reason="Default" />
-    <disable_collisions link1="LShoulder" link2="helmet" reason="Never" />
     <disable_collisions link1="LShoulder" link2="l_ankle" reason="Never" />
     <disable_collisions link1="LShoulder" link2="l_sole" reason="Never" />
     <disable_collisions link1="LShoulder" link2="l_wrist" reason="Never" />
     <disable_collisions link1="LShoulder" link2="neck" reason="Never" />
-    <disable_collisions link1="LShoulder" link2="odroid" reason="Never" />
-    <disable_collisions link1="LShoulder" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="LShoulder" link2="r_ankle" reason="Never" />
     <disable_collisions link1="LShoulder" link2="r_sole" reason="Never" />
     <disable_collisions link1="LShoulder" link2="r_wrist" reason="Never" />
@@ -310,6 +300,7 @@
     <disable_collisions link1="LShoulderDummy" link2="LThumb1" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="LThumb2" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RElbowDummy" reason="Never" />
+    <disable_collisions link1="LShoulderDummy" link2="RFinger21" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RHip" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RHipDummy" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RHipYaw" reason="Never" />
@@ -319,27 +310,30 @@
     <disable_collisions link1="LShoulderDummy" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RThumb2" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="gaze" reason="Default" />
+    <disable_collisions link1="LShoulderDummy" link2="l_ankle" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="l_wrist" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="neck" reason="Never" />
-    <disable_collisions link1="LShoulderDummy" link2="odroid" reason="Never" />
-    <disable_collisions link1="LShoulderDummy" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="r_ankle" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="r_sole" reason="Never" />
-    <disable_collisions link1="LShoulderDummy" link2="r_wrist" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="torso" reason="Default" />
     <disable_collisions link1="LThumb1" link2="LThumb2" reason="Adjacent" />
+    <disable_collisions link1="LThumb1" link2="RElbowDummy" reason="Never" />
     <disable_collisions link1="LThumb1" link2="RHip" reason="Never" />
     <disable_collisions link1="LThumb1" link2="RHipDummy" reason="Never" />
     <disable_collisions link1="LThumb1" link2="RKnee" reason="Never" />
     <disable_collisions link1="LThumb1" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LThumb1" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="LThumb1" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LThumb1" link2="l_wrist" reason="Adjacent" />
     <disable_collisions link1="LThumb1" link2="neck" reason="Never" />
     <disable_collisions link1="LThumb1" link2="r_ankle" reason="Never" />
     <disable_collisions link1="LThumb1" link2="r_sole" reason="Never" />
+    <disable_collisions link1="LThumb2" link2="RElbowDummy" reason="Never" />
+    <disable_collisions link1="LThumb2" link2="RHip" reason="Never" />
     <disable_collisions link1="LThumb2" link2="RKnee" reason="Never" />
     <disable_collisions link1="LThumb2" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LThumb2" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="LThumb2" link2="RThumb1" reason="Never" />
     <disable_collisions link1="LThumb2" link2="l_wrist" reason="Default" />
     <disable_collisions link1="LThumb2" link2="neck" reason="Never" />
     <disable_collisions link1="LThumb2" link2="r_ankle" reason="Never" />
@@ -352,12 +346,12 @@
     <disable_collisions link1="RElbowDummy" link2="RFinger23" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="RShoulderDummy" reason="Adjacent" />
+    <disable_collisions link1="RElbowDummy" link2="RThumb1" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="RThumb2" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="l_sole" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="neck" reason="Never" />
-    <disable_collisions link1="RElbowDummy" link2="odroid" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="r_wrist" reason="Adjacent" />
-    <disable_collisions link1="RElbowDummy" link2="torso" reason="Never" />
     <disable_collisions link1="RFinger11" link2="RFinger12" reason="Adjacent" />
     <disable_collisions link1="RFinger11" link2="RFinger13" reason="Never" />
     <disable_collisions link1="RFinger11" link2="RFinger21" reason="Never" />
@@ -367,12 +361,9 @@
     <disable_collisions link1="RFinger11" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RFinger11" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RFinger11" link2="RThumb2" reason="Default" />
-    <disable_collisions link1="RFinger11" link2="LElbowDummy" reason="Never" />
+    <disable_collisions link1="RFinger11" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RFinger11" link2="LFinger13" reason="Never" />
-    <disable_collisions link1="RFinger11" link2="LKnee" reason="Never" />
-    <disable_collisions link1="RFinger11" link2="LShoulder" reason="Never" />
-    <disable_collisions link1="RFinger11" link2="r_wrist" reason="Adjacent" />
     <disable_collisions link1="RFinger11" link2="neck" reason="Never" />
+    <disable_collisions link1="RFinger11" link2="r_wrist" reason="Adjacent" />
     <disable_collisions link1="RFinger12" link2="RFinger13" reason="Adjacent" />
     <disable_collisions link1="RFinger12" link2="RFinger21" reason="Never" />
     <disable_collisions link1="RFinger12" link2="RFinger22" reason="Never" />
@@ -381,11 +372,9 @@
     <disable_collisions link1="RFinger12" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RFinger12" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RFinger12" link2="RThumb2" reason="Default" />
-    <disable_collisions link1="RFinger12" link2="LFinger13" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RFinger12" link2="LShoulder" reason="Never" />
-    <disable_collisions link1="RFinger12" link2="r_wrist" reason="Default" />
     <disable_collisions link1="RFinger12" link2="neck" reason="Never" />
-    <disable_collisions link1="RFinger12" link2="odroid" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="r_wrist" reason="Default" />
     <disable_collisions link1="RFinger13" link2="RFinger21" reason="Never" />
     <disable_collisions link1="RFinger13" link2="RFinger22" reason="Never" />
     <disable_collisions link1="RFinger13" link2="RFinger23" reason="Never" />
@@ -393,90 +382,70 @@
     <disable_collisions link1="RFinger13" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RFinger13" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RFinger13" link2="RThumb2" reason="Default" />
-    <disable_collisions link1="RFinger13" link2="LShoulder" reason="Never" />
+    <disable_collisions link1="RFinger13" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RFinger13" link2="r_wrist" reason="Default" />
     <disable_collisions link1="RFinger13" link2="neck" reason="Never" />
+    <disable_collisions link1="RFinger13" link2="r_wrist" reason="Default" />
     <disable_collisions link1="RFinger21" link2="RFinger22" reason="Adjacent" />
     <disable_collisions link1="RFinger21" link2="RFinger23" reason="Never" />
     <disable_collisions link1="RFinger21" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RFinger21" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RFinger21" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RFinger21" link2="RThumb2" reason="Never" />
-    <disable_collisions link1="RFinger21" link2="LShoulder" reason="Never" />
+    <disable_collisions link1="RFinger21" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RFinger21" link2="LShoulderDummy" reason="Never" />
-    <disable_collisions link1="RFinger21" link2="r_wrist" reason="Adjacent" />
     <disable_collisions link1="RFinger21" link2="neck" reason="Never" />
-    <disable_collisions link1="RFinger21" link2="l_sole" reason="Never" />
+    <disable_collisions link1="RFinger21" link2="r_wrist" reason="Adjacent" />
     <disable_collisions link1="RFinger22" link2="RFinger23" reason="Adjacent" />
     <disable_collisions link1="RFinger22" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RFinger22" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RFinger22" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RFinger22" link2="RThumb2" reason="Never" />
-    <disable_collisions link1="RFinger22" link2="LShoulder" reason="Never" />
+    <disable_collisions link1="RFinger22" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RFinger22" link2="r_wrist" reason="Never" />
     <disable_collisions link1="RFinger22" link2="neck" reason="Never" />
+    <disable_collisions link1="RFinger22" link2="r_wrist" reason="Never" />
     <disable_collisions link1="RFinger23" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RFinger23" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RFinger23" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RFinger23" link2="RThumb2" reason="Never" />
-    <disable_collisions link1="RFinger23" link2="LFinger11" reason="Never" />
+    <disable_collisions link1="RFinger23" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RFinger23" link2="LShoulder" reason="Never" />
-    <disable_collisions link1="RFinger23" link2="r_wrist" reason="Never" />
     <disable_collisions link1="RFinger23" link2="neck" reason="Never" />
+    <disable_collisions link1="RFinger23" link2="r_wrist" reason="Never" />
     <disable_collisions link1="RHip" link2="RHipDummy" reason="Adjacent" />
     <disable_collisions link1="RHip" link2="RHipYaw" reason="Adjacent" />
     <disable_collisions link1="RHip" link2="RKnee" reason="Never" />
     <disable_collisions link1="RHip" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RHip" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RHip" link2="gaze" reason="Never" />
-    <disable_collisions link1="RHip" link2="helmet" reason="Never" />
     <disable_collisions link1="RHip" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RHip" link2="neck" reason="Never" />
-    <disable_collisions link1="RHip" link2="odroid" reason="Never" />
-    <disable_collisions link1="RHip" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="RHip" link2="r_ankle" reason="Never" />
     <disable_collisions link1="RHip" link2="torso" reason="Never" />
     <disable_collisions link1="RHipDummy" link2="RHipYaw" reason="Default" />
     <disable_collisions link1="RHipDummy" link2="RKnee" reason="Adjacent" />
     <disable_collisions link1="RHipDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RHipDummy" link2="gaze" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="helmet" reason="Never" />
     <disable_collisions link1="RHipDummy" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RHipDummy" link2="neck" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="odroid" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="RHipDummy" link2="r_ankle" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="torso" reason="Default" />
     <disable_collisions link1="RHipYaw" link2="RKnee" reason="Never" />
     <disable_collisions link1="RHipYaw" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RHipYaw" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="RHipYaw" link2="gaze" reason="Never" />
-    <disable_collisions link1="RHipYaw" link2="helmet" reason="Never" />
-    <disable_collisions link1="RHipYaw" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RHipYaw" link2="neck" reason="Never" />
-    <disable_collisions link1="RHipYaw" link2="odroid" reason="Never" />
-    <disable_collisions link1="RHipYaw" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="RHipYaw" link2="r_ankle" reason="Never" />
     <disable_collisions link1="RHipYaw" link2="torso" reason="Adjacent" />
     <disable_collisions link1="RKnee" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RKnee" link2="gaze" reason="Never" />
-    <disable_collisions link1="RKnee" link2="helmet" reason="Never" />
     <disable_collisions link1="RKnee" link2="l_wrist" reason="Never" />
     <disable_collisions link1="RKnee" link2="neck" reason="Never" />
-    <disable_collisions link1="RKnee" link2="odroid" reason="Never" />
-    <disable_collisions link1="RKnee" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="RKnee" link2="r_ankle" reason="Adjacent" />
-    <disable_collisions link1="RKnee" link2="r_sole" reason="Adjacent" />
+    <disable_collisions link1="RKnee" link2="r_sole" reason="Never" />
     <disable_collisions link1="RShoulder" link2="RShoulderDummy" reason="Adjacent" />
     <disable_collisions link1="RShoulder" link2="RThumb1" reason="Never" />
     <disable_collisions link1="RShoulder" link2="RThumb2" reason="Never" />
-    <disable_collisions link1="RShoulder" link2="helmet" reason="Never" />
     <disable_collisions link1="RShoulder" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RShoulder" link2="l_sole" reason="Never" />
     <disable_collisions link1="RShoulder" link2="l_wrist" reason="Never" />
     <disable_collisions link1="RShoulder" link2="neck" reason="Never" />
-    <disable_collisions link1="RShoulder" link2="odroid" reason="Never" />
-    <disable_collisions link1="RShoulder" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="RShoulder" link2="r_ankle" reason="Never" />
     <disable_collisions link1="RShoulder" link2="r_wrist" reason="Never" />
     <disable_collisions link1="RShoulder" link2="torso" reason="Adjacent" />
@@ -486,61 +455,32 @@
     <disable_collisions link1="RShoulderDummy" link2="l_sole" reason="Never" />
     <disable_collisions link1="RShoulderDummy" link2="l_wrist" reason="Never" />
     <disable_collisions link1="RShoulderDummy" link2="neck" reason="Never" />
-    <disable_collisions link1="RShoulderDummy" link2="odroid" reason="Never" />
     <disable_collisions link1="RShoulderDummy" link2="r_wrist" reason="Never" />
     <disable_collisions link1="RThumb1" link2="RThumb2" reason="Adjacent" />
     <disable_collisions link1="RThumb1" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RThumb1" link2="l_sole" reason="Never" />
     <disable_collisions link1="RThumb1" link2="neck" reason="Never" />
     <disable_collisions link1="RThumb1" link2="r_wrist" reason="Adjacent" />
     <disable_collisions link1="RThumb2" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RThumb2" link2="neck" reason="Never" />
     <disable_collisions link1="RThumb2" link2="r_wrist" reason="Default" />
-    <disable_collisions link1="gaze" link2="helmet" reason="Adjacent" />
     <disable_collisions link1="gaze" link2="l_ankle" reason="Never" />
     <disable_collisions link1="gaze" link2="l_sole" reason="Never" />
     <disable_collisions link1="gaze" link2="neck" reason="Adjacent" />
-    <disable_collisions link1="gaze" link2="odroid_bag" reason="Default" />
     <disable_collisions link1="gaze" link2="r_ankle" reason="Never" />
     <disable_collisions link1="gaze" link2="r_sole" reason="Never" />
     <disable_collisions link1="gaze" link2="torso" reason="Default" />
-    <disable_collisions link1="helmet" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="helmet" link2="l_sole" reason="Never" />
-    <disable_collisions link1="helmet" link2="neck" reason="Never" />
-    <disable_collisions link1="helmet" link2="odroid" reason="Never" />
-    <disable_collisions link1="helmet" link2="odroid_bag" reason="Never" />
-    <disable_collisions link1="helmet" link2="r_ankle" reason="Never" />
-    <disable_collisions link1="helmet" link2="r_sole" reason="Never" />
-    <disable_collisions link1="helmet" link2="torso" reason="Never" />
     <disable_collisions link1="l_ankle" link2="l_sole" reason="Adjacent" />
     <disable_collisions link1="l_ankle" link2="neck" reason="Never" />
-    <disable_collisions link1="l_ankle" link2="odroid" reason="Never" />
-    <disable_collisions link1="l_ankle" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="l_ankle" link2="r_wrist" reason="Never" />
-    <disable_collisions link1="l_ankle" link2="r_sole" reason="Never" />
     <disable_collisions link1="l_sole" link2="neck" reason="Never" />
-    <disable_collisions link1="l_sole" link2="odroid" reason="Never" />
-    <disable_collisions link1="l_sole" link2="odroid_bag" reason="Never" />
-    <disable_collisions link1="l_sole" link2="RKnee" reason="Adjacent" />
     <disable_collisions link1="l_wrist" link2="neck" reason="Never" />
     <disable_collisions link1="l_wrist" link2="r_ankle" reason="Never" />
     <disable_collisions link1="l_wrist" link2="r_sole" reason="Never" />
-    <disable_collisions link1="neck" link2="odroid" reason="Never" />
-    <disable_collisions link1="neck" link2="odroid_bag" reason="Never" />
     <disable_collisions link1="neck" link2="r_ankle" reason="Never" />
     <disable_collisions link1="neck" link2="r_sole" reason="Never" />
     <disable_collisions link1="neck" link2="r_wrist" reason="Never" />
     <disable_collisions link1="neck" link2="torso" reason="Adjacent" />
-    <disable_collisions link1="odroid" link2="odroid_bag" reason="Adjacent" />
-    <disable_collisions link1="odroid" link2="r_ankle" reason="Never" />
-    <disable_collisions link1="odroid" link2="r_sole" reason="Never" />
-    <disable_collisions link1="odroid" link2="torso" reason="Never" />
-    <disable_collisions link1="odroid_bag" link2="r_ankle" reason="Never" />
-    <disable_collisions link1="odroid_bag" link2="r_sole" reason="Never" />
-    <disable_collisions link1="odroid_bag" link2="torso" reason="Adjacent" />
     <disable_collisions link1="r_ankle" link2="r_sole" reason="Adjacent" />
     <disable_collisions link1="r_ankle" link2="torso" reason="Never" />
-    <disable_collisions link1="r_ankle" link2="l_sole" reason="Never" />
     <disable_collisions link1="r_sole" link2="torso" reason="Never" />
-    <disable_collisions link1="r_sole" link2="LKnee" reason="Adjacent" />
 </robot>

nao_dcm_common/nao_dcm_description/urdf/modules/helmet.xacro

@@ -1,5 +1,22 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+	<!-- XACRO MACROS FOR VISUALS AND COLLISIONS -->
+	<xacro:macro name="visuals_collisions_helmet" params="">
+		<visual>
+			<origin xyz="0 0 0" rpy="0 -0 0"/>
+			<geometry>
+				<mesh filename="package://nao_dcm_description/meshes/helmet.dae"  scale="${scale} ${scale} ${scale}"/>
+			</geometry>
+			<material name="LightGrey"/>
+		</visual>
+		<!-- <collision>
+			<origin xyz="0 0 0" rpy="0 -0 0"/>
+			<geometry>
+				<mesh filename="package://nao_dcm_description/meshes/helmet.dae"  scale="${scale} ${scale} ${scale}"/>
+			</geometry>
+			<material name="LightGrey"/>
+		</collision> -->
+	</xacro:macro>
 	<joint name="head-helmet" type="fixed">
         <parent link="gaze"/>
         <child link="helmet"/>

nao_dcm_common/nao_dcm_description/urdf/modules/helmet_xtion.xacro

@@ -10,9 +10,10 @@
 			<material name="LightGrey"/>
 		</visual>
 		<collision>
-			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<origin xyz="0 0 0.05" rpy="${PI_2} 0 0"/>
 			<geometry>
-				<mesh filename="package://nao_dcm_description/meshes/helmet_xtion.dae"/>
+				<!-- <mesh filename="package://nao_dcm_description/meshes/helmet_xtion.dae"/> -->
+        <cylinder radius="0.01" length="0.14"/>
 			</geometry>
 			<material name="LightGrey"/>
 		</collision>
@@ -33,6 +34,7 @@
     <link name="xtion"/>
 
     <!-- GAZEBO SPECIFIC FOR XTION -->
+    <xacro:if value="${use_gazebo}">
       <gazebo reference="helmet-xtion">
       	<sensor type="depth" name="xtion">
           <always_on>true</always_on>
@@ -69,4 +71,5 @@
           </plugin>
         </sensor>
   	</gazebo>
+  </xacro:if>
 </robot>

nao_dcm_common/nao_dcm_description/urdf/modules/odroid_bag.xacro

@@ -10,9 +10,10 @@
 			<material name="LightGrey"/>
 		</visual>
 		<collision>
-			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<origin xyz="-0.02 0 0" rpy="0 0 0"/>
 			<geometry>
-				<mesh filename="package://nao_dcm_description/meshes/odroid_bag.dae"  scale="${scale} ${scale} ${scale}"/>
+				<!-- <mesh filename="package://nao_dcm_description/meshes/odroid_bag.dae"  scale="${scale} ${scale} ${scale}"/> -->
+				<box size="0.01 0.01 0.01"/>
 			</geometry>
 			<material name="LightGrey"/>
 		</collision>
@@ -27,9 +28,10 @@
 			<material name="LightGrey"/>
 		</visual>
 		<collision>
-			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<origin xyz="0 0.01 0" rpy="0 0 0"/>
 			<geometry>
-				<mesh filename="package://nao_dcm_description/meshes/odroid.dae"  scale="${scale} ${scale} ${scale}"/>
+				<!-- <mesh filename="package://nao_dcm_description/meshes/odroid.dae"  scale="${scale} ${scale} ${scale}"/> -->
+				<box size="0.01 0.01 0.01"/>
 			</geometry>
 			<material name="LightGrey"/>
 		</collision>

nao_dcm_common/nao_dcm_description/urdf/modules/robot.xacro

@@ -11,6 +11,7 @@
 	<xacro:property name="use_xtion" value="true" />
 	<xacro:property name="use_odroid" value="true" />
 	<xacro:property name="use_hector_imu" value="false" />
+	<xacro:property name="use_gazebo" value="true" />
 	<xacro:property name="effort_type1" value="0.0161" />
 	<xacro:property name="effort_type2" value="0.0049" />
 	<xacro:property name="effort_type3" value="0.0062" />
@@ -30,7 +31,9 @@
 	</xacro:unless>
 
 	<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_visuals_collisions.xacro" />
+	<xacro:if value="${use_gazebo}">
 		<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_gazebo.xacro" />
+	</xacro:if>
 
 	<!-- Load xacro modules/parts of the robot -->
 	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/torso.xacro" />

nao_dcm_common/nao_dcm_description/urdf/nao_dcm_visuals_collisions.xacro

@@ -27,23 +27,6 @@
 
     <!-- XACRO MACROS FOR VISUALS AND COLLISIONS -->
 
-    <xacro:macro name="visuals_collisions_helmet" params="">
-		<visual>
-			<origin xyz="0 0 0" rpy="0 -0 0"/>
-			<geometry>
-				<mesh filename="package://nao_dcm_description/meshes/helmet.dae"  scale="${scale} ${scale} ${scale}"/>
-			</geometry>
-			<material name="LightGrey"/>
-		</visual>
-		<collision>
-			<origin xyz="0 0 0" rpy="0 -0 0"/>
-			<geometry>
-				<mesh filename="package://nao_dcm_description/meshes/helmet.dae"  scale="${scale} ${scale} ${scale}"/>
-			</geometry>
-			<material name="LightGrey"/>
-		</collision>
-	</xacro:macro>
-
 	<xacro:macro name="visuals_collisions_torso" params="">
 		<xacro:unless value="${nao_meshes}">
 			<visual>