diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H21.srdf b/nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H21.srdf
index 6a2ebbf..5c7ae24 100644
--- a/nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H21.srdf
+++ b/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="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="l_ankle" reason="Never" />
     <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="LShoulder" reason="Never" />
-    <disable_collisions link1="RFinger12" link2="r_wrist" reason="Default" />
+    <disable_collisions link1="RFinger12" link2="l_ankle" reason="Never" />
     <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="r_wrist" reason="Default" />
+    <disable_collisions link1="RFinger13" link2="l_ankle" reason="Never" />
     <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="LShoulderDummy" reason="Never" />
-    <disable_collisions link1="RFinger21" link2="r_wrist" reason="Adjacent" />
+    <disable_collisions link1="RFinger21" link2="l_ankle" reason="Never" />
     <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="r_wrist" reason="Never" />
+    <disable_collisions link1="RFinger22" link2="l_ankle" 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="LShoulder" reason="Never" />
-    <disable_collisions link1="RFinger23" link2="r_wrist" reason="Never" />
+    <disable_collisions link1="RFinger23" link2="l_ankle" 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>
diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H25.srdf b/nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H25.srdf
index c3b9bd5..b020273 100644
--- a/nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H25.srdf
+++ b/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="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="l_ankle" reason="Never" />
     <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="LShoulder" reason="Never" />
-    <disable_collisions link1="RFinger12" link2="r_wrist" reason="Default" />
+    <disable_collisions link1="RFinger12" link2="l_ankle" reason="Never" />
     <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="r_wrist" reason="Default" />
+    <disable_collisions link1="RFinger13" link2="l_ankle" reason="Never" />
     <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="LShoulderDummy" reason="Never" />
-    <disable_collisions link1="RFinger21" link2="r_wrist" reason="Adjacent" />
+    <disable_collisions link1="RFinger21" link2="l_ankle" reason="Never" />
     <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="r_wrist" reason="Never" />
+    <disable_collisions link1="RFinger22" link2="l_ankle" 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="LShoulder" reason="Never" />
-    <disable_collisions link1="RFinger23" link2="r_wrist" reason="Never" />
+    <disable_collisions link1="RFinger23" link2="l_ankle" 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>
diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/helmet.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/helmet.xacro
index 37c4a03..b4f9c30 100644
--- a/nao_dcm_common/nao_dcm_description/urdf/modules/helmet.xacro
+++ b/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"/>
diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/helmet_xtion.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/helmet_xtion.xacro
index c179df0..c97a55f 100644
--- a/nao_dcm_common/nao_dcm_description/urdf/modules/helmet_xtion.xacro
+++ b/nao_dcm_common/nao_dcm_description/urdf/modules/helmet_xtion.xacro
@@ -1,8 +1,8 @@
 <?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_xtion" params="">
-    	<visual>
+  <!-- XACRO MACROS FOR VISUALS AND COLLISIONS -->
+  <xacro:macro name="visuals_collisions_helmet_xtion" params="">
+    <visual>
 			<origin xyz="0 0 0" rpy="0 0 0"/>
 			<geometry>
 				<mesh filename="package://nao_dcm_description/meshes/helmet_xtion.dae"/>
@@ -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,40 +34,42 @@
     <link name="xtion"/>
 
     <!-- GAZEBO SPECIFIC FOR XTION -->
-    <gazebo reference="helmet-xtion">
-    	<sensor type="depth" name="xtion">
-        <always_on>true</always_on>
-        <update_rate>30.0</update_rate>
-        <camera>
-          <horizontal_fov>${60.0*PI/180.0}</horizontal_fov>
-          <image>
-            <format>R8G8B8</format>
-            <width>640</width>
-            <height>480</height>
-          </image>
-          <clip>
-            <near>0.05</near>
-            <far>8.0</far>
-          </clip>
-        </camera>
-        <plugin name="xtion_camera_controller" filename="libgazebo_ros_openni_kinect.so">
-          <cameraName>xtion</cameraName>
-          <alwaysOn>true</alwaysOn>
-          <updateRate>30</updateRate>
-          <imageTopicName>xtion/rgb/image_raw</imageTopicName>
-          <depthImageTopicName>xtion/depth/image_raw</depthImageTopicName>
-          <pointCloudTopicName>xtion/depth/points</pointCloudTopicName>
-          <cameraInfoTopicName>xtion/rgb/camera_info</cameraInfoTopicName>
-          <depthImageCameraInfoTopicName>xtion/depth/camera_info</depthImageCameraInfoTopicName>
-          <frameName>xtion</frameName>
-          <baseline>0.1</baseline>
-          <distortion_k1>0.0</distortion_k1>
-          <distortion_k2>0.0</distortion_k2>
-          <distortion_k3>0.0</distortion_k3>
-          <distortion_t1>0.0</distortion_t1>
-          <distortion_t2>0.0</distortion_t2>
-          <pointCloudCutoff>0.4</pointCloudCutoff>
-        </plugin>
-      </sensor>
-	</gazebo>
+    <xacro:if value="${use_gazebo}">
+      <gazebo reference="helmet-xtion">
+      	<sensor type="depth" name="xtion">
+          <always_on>true</always_on>
+          <update_rate>30.0</update_rate>
+          <camera>
+            <horizontal_fov>${60.0*PI/180.0}</horizontal_fov>
+            <image>
+              <format>R8G8B8</format>
+              <width>640</width>
+              <height>480</height>
+            </image>
+            <clip>
+              <near>0.05</near>
+              <far>8.0</far>
+            </clip>
+          </camera>
+          <plugin name="xtion_camera_controller" filename="libgazebo_ros_openni_kinect.so">
+            <cameraName>xtion</cameraName>
+            <alwaysOn>true</alwaysOn>
+            <updateRate>30</updateRate>
+            <imageTopicName>xtion/rgb/image_raw</imageTopicName>
+            <depthImageTopicName>xtion/depth/image_raw</depthImageTopicName>
+            <pointCloudTopicName>xtion/depth/points</pointCloudTopicName>
+            <cameraInfoTopicName>xtion/rgb/camera_info</cameraInfoTopicName>
+            <depthImageCameraInfoTopicName>xtion/depth/camera_info</depthImageCameraInfoTopicName>
+            <frameName>xtion</frameName>
+            <baseline>0.1</baseline>
+            <distortion_k1>0.0</distortion_k1>
+            <distortion_k2>0.0</distortion_k2>
+            <distortion_k3>0.0</distortion_k3>
+            <distortion_t1>0.0</distortion_t1>
+            <distortion_t2>0.0</distortion_t2>
+            <pointCloudCutoff>0.4</pointCloudCutoff>
+          </plugin>
+        </sensor>
+  	</gazebo>
+  </xacro:if>
 </robot>
\ No newline at end of file
diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/odroid_bag.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/odroid_bag.xacro
index ada5b22..016333e 100644
--- a/nao_dcm_common/nao_dcm_description/urdf/modules/odroid_bag.xacro
+++ b/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>
diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/robot.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/robot.xacro
index b23eb6e..846645d 100644
--- a/nao_dcm_common/nao_dcm_description/urdf/modules/robot.xacro
+++ b/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:include filename="$(find nao_dcm_description)/urdf/nao_dcm_gazebo.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" />
diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_visuals_collisions.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_visuals_collisions.xacro
index d6c4871..ecd5023 100644
--- a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_visuals_collisions.xacro
+++ b/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>