diff --git a/nao_dcm_apps/nao_dcm_moveit_config/.setup_assistant b/nao_dcm_apps/nao_dcm_moveit_config/.setup_assistant
index 4c6b6fb..2717be2 100644
--- a/nao_dcm_apps/nao_dcm_moveit_config/.setup_assistant
+++ b/nao_dcm_apps/nao_dcm_moveit_config/.setup_assistant
@@ -5,4 +5,4 @@ moveit_setup_assistant_config:
   SRDF:
     relative_path: config/nao_robot_v4_H25.srdf
   CONFIG:
-    generated_timestamp: 1398368537
\ No newline at end of file
+    generated_timestamp: 1400500416
\ No newline at end of file
diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/fake_controllers.yaml b/nao_dcm_apps/nao_dcm_moveit_config/config/fake_controllers.yaml
index d26fef3..4988634 100644
--- a/nao_dcm_apps/nao_dcm_moveit_config/config/fake_controllers.yaml
+++ b/nao_dcm_apps/nao_dcm_moveit_config/config/fake_controllers.yaml
@@ -39,4 +39,47 @@ controller_list:
   - name: fake_LeftHand_controller
     joints:
       - LWristYaw
-      - LHand
\ No newline at end of file
+      - LHand
+  - name: fake_RightArmHand_controller
+    joints:
+      - RShoulderPitch
+      - RShoulderRoll
+      - RElbowYaw
+      - RElbowRoll
+      - RWristYaw
+      - RHand
+  - name: fake_LeftArmHand_controller
+    joints:
+      - LShoulderPitch
+      - LShoulderRoll
+      - LElbowYaw
+      - LElbowRoll
+      - LWristYaw
+      - LHand
+  - name: fake_Body_controller
+    joints:
+      - LHipYawPitch
+      - LHipRoll
+      - LHipPitch
+      - LKneePitch
+      - LAnklePitch
+      - LAnkleRoll
+      - RHipRoll
+      - RHipPitch
+      - RKneePitch
+      - RAnklePitch
+      - RAnkleRoll
+      - HeadYaw
+      - HeadPitch
+      - LShoulderPitch
+      - LShoulderRoll
+      - LElbowYaw
+      - LElbowRoll
+      - LWristYaw
+      - LHand
+      - RShoulderPitch
+      - RShoulderRoll
+      - RElbowYaw
+      - RElbowRoll
+      - RWristYaw
+      - RHand
\ No newline at end of file
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 74c8139..43adeb5 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
@@ -71,83 +71,172 @@
     <passive_joint name="virtual_joint" />
     <passive_joint name="RHipYawPitch" />
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="LElbowDummy" link2="LShoulder" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="LShoulderDummy" reason="Adjacent" />
     <disable_collisions link1="LElbowDummy" link2="RElbowDummy" 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" />
     <disable_collisions link1="LElbowDummy" link2="RKnee" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="RShoulderDummy" reason="Never" />
-    <disable_collisions link1="LElbowDummy" link2="l_ankle" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="l_wrist" reason="Adjacent" />
+    <disable_collisions link1="LElbowDummy" link2="neck" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="r_ankle" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="r_sole" reason="Never" />
-    <disable_collisions link1="LElbowDummy" link2="r_wrist" reason="Never" />
+    <disable_collisions link1="LHip" link2="LHipDummy" reason="Adjacent" />
+    <disable_collisions link1="LHip" link2="LHipYaw" reason="Adjacent" />
+    <disable_collisions link1="LHip" link2="LKnee" reason="Never" />
+    <disable_collisions link1="LHip" link2="LShoulder" reason="Never" />
+    <disable_collisions link1="LHip" link2="LShoulderDummy" reason="Never" />
+    <disable_collisions link1="LHip" link2="RElbowDummy" reason="Never" />
+    <disable_collisions link1="LHip" link2="RHip" reason="Never" />
+    <disable_collisions link1="LHip" link2="RHipDummy" reason="Never" />
+    <disable_collisions link1="LHip" link2="RHipYaw" reason="Never" />
+    <disable_collisions link1="LHip" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LHip" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="LHip" link2="gaze" reason="Never" />
+    <disable_collisions link1="LHip" link2="l_ankle" reason="Never" />
+    <disable_collisions link1="LHip" link2="neck" reason="Never" />
+    <disable_collisions link1="LHip" link2="r_ankle" 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="LShoulderDummy" reason="Never" />
+    <disable_collisions link1="LHipDummy" link2="LShoulder" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="RElbowDummy" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="RHipDummy" reason="Adjacent" />
+    <disable_collisions link1="LHipDummy" link2="RHip" reason="Never" />
+    <disable_collisions link1="LHipDummy" link2="RHipYaw" reason="Never" />
+    <disable_collisions link1="LHipDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="gaze" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="l_sole" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="r_ankle" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="r_wrist" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="torso" reason="Adjacent" />
-    <disable_collisions link1="LKnee" link2="LShoulderDummy" reason="Never" />
+    <disable_collisions link1="LHipDummy" link2="neck" reason="Never" />
+    <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="RHip" reason="Never" />
+    <disable_collisions link1="LHipYaw" link2="RHipDummy" reason="Never" />
+    <disable_collisions link1="LHipYaw" link2="RHipYaw" reason="Adjacent" />
+    <disable_collisions link1="LHipYaw" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LHipYaw" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="LHipYaw" link2="gaze" reason="Never" />
+    <disable_collisions link1="LHipYaw" link2="neck" 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="RShoulder" reason="Never" />
     <disable_collisions link1="LKnee" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LKnee" link2="gaze" 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="l_sole" reason="Default" />
+    <disable_collisions link1="LKnee" link2="neck" reason="Never" />
     <disable_collisions link1="LKnee" link2="r_wrist" reason="Never" />
     <disable_collisions link1="LKnee" link2="torso" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="LShoulderDummy" reason="Adjacent" />
+    <disable_collisions link1="LShoulder" link2="RElbowDummy" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RHip" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RHipDummy" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RHipYaw" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RKnee" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RShoulderDummy" 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="r_ankle" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="r_sole" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="r_wrist" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="torso" reason="Adjacent" />
     <disable_collisions link1="LShoulderDummy" link2="RElbowDummy" 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" />
     <disable_collisions link1="LShoulderDummy" link2="RKnee" reason="Never" />
+    <disable_collisions link1="LShoulderDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="l_ankle" reason="Never" />
+    <disable_collisions link1="LShoulderDummy" link2="l_sole" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="l_wrist" reason="Never" />
+    <disable_collisions link1="LShoulderDummy" link2="neck" 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="Adjacent" />
+    <disable_collisions link1="LShoulderDummy" link2="torso" reason="Default" />
+    <disable_collisions link1="RElbowDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="RShoulderDummy" reason="Adjacent" />
     <disable_collisions link1="RElbowDummy" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="l_sole" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="l_wrist" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="neck" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="r_ankle" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="r_wrist" reason="Adjacent" />
+    <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="l_ankle" reason="Never" />
+    <disable_collisions link1="RHip" link2="neck" 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="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="RHipDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RHipDummy" link2="gaze" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="l_wrist" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="r_ankle" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="r_sole" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="torso" reason="Adjacent" />
-    <disable_collisions link1="RKnee" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="RHipDummy" link2="neck" reason="Never" />
+    <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="neck" 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="l_wrist" reason="Never" />
+    <disable_collisions link1="RKnee" link2="neck" reason="Never" />
     <disable_collisions link1="RKnee" link2="r_ankle" reason="Adjacent" />
-    <disable_collisions link1="RKnee" link2="r_sole" reason="Never" />
+    <disable_collisions link1="RKnee" link2="r_sole" reason="Default" />
     <disable_collisions link1="RKnee" link2="torso" reason="Never" />
+    <disable_collisions link1="RShoulder" link2="RShoulderDummy" reason="Adjacent" />
+    <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="r_ankle" reason="Never" />
+    <disable_collisions link1="RShoulder" link2="r_sole" reason="Never" />
+    <disable_collisions link1="RShoulder" link2="r_wrist" reason="Never" />
+    <disable_collisions link1="RShoulder" link2="torso" reason="Adjacent" />
     <disable_collisions link1="RShoulderDummy" link2="l_ankle" reason="Never" />
     <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="r_ankle" reason="Never" />
+    <disable_collisions link1="RShoulderDummy" link2="r_sole" reason="Never" />
     <disable_collisions link1="RShoulderDummy" link2="r_wrist" reason="Never" />
-    <disable_collisions link1="RShoulderDummy" link2="torso" reason="Adjacent" />
+    <disable_collisions link1="RShoulderDummy" link2="torso" reason="Default" />
     <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="r_ankle" reason="Never" />
     <disable_collisions link1="gaze" link2="r_sole" reason="Never" />
-    <disable_collisions link1="gaze" link2="torso" reason="Adjacent" />
     <disable_collisions link1="l_ankle" link2="l_sole" reason="Adjacent" />
-    <disable_collisions link1="l_ankle" link2="l_wrist" reason="Never" />
+    <disable_collisions link1="l_ankle" link2="neck" reason="Never" />
     <disable_collisions link1="l_ankle" link2="r_wrist" reason="Never" />
     <disable_collisions link1="l_ankle" link2="torso" reason="Never" />
+    <disable_collisions link1="l_sole" link2="neck" reason="Never" />
     <disable_collisions link1="l_sole" link2="r_wrist" reason="Never" />
+    <disable_collisions link1="l_sole" link2="torso" reason="Never" />
+    <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="l_wrist" link2="r_wrist" reason="Default" />
+    <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="r_ankle" link2="r_sole" reason="Adjacent" />
     <disable_collisions link1="r_ankle" link2="torso" reason="Never" />
+    <disable_collisions link1="r_sole" link2="torso" reason="Never" />
 </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 522152d..5b61092 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
@@ -102,83 +102,172 @@
     <passive_joint name="virtual_joint" />
     <passive_joint name="RHipYawPitch" />
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="LElbowDummy" link2="LShoulder" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="LShoulderDummy" reason="Adjacent" />
     <disable_collisions link1="LElbowDummy" link2="RElbowDummy" 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" />
     <disable_collisions link1="LElbowDummy" link2="RKnee" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="RShoulderDummy" reason="Never" />
-    <disable_collisions link1="LElbowDummy" link2="l_ankle" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="l_wrist" reason="Adjacent" />
+    <disable_collisions link1="LElbowDummy" link2="neck" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="r_ankle" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="r_sole" reason="Never" />
-    <disable_collisions link1="LElbowDummy" link2="r_wrist" reason="Never" />
+    <disable_collisions link1="LHip" link2="LHipDummy" reason="Adjacent" />
+    <disable_collisions link1="LHip" link2="LHipYaw" reason="Adjacent" />
+    <disable_collisions link1="LHip" link2="LKnee" reason="Never" />
+    <disable_collisions link1="LHip" link2="LShoulder" reason="Never" />
+    <disable_collisions link1="LHip" link2="LShoulderDummy" reason="Never" />
+    <disable_collisions link1="LHip" link2="RElbowDummy" reason="Never" />
+    <disable_collisions link1="LHip" link2="RHip" reason="Never" />
+    <disable_collisions link1="LHip" link2="RHipDummy" reason="Never" />
+    <disable_collisions link1="LHip" link2="RHipYaw" reason="Never" />
+    <disable_collisions link1="LHip" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LHip" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="LHip" link2="gaze" reason="Never" />
+    <disable_collisions link1="LHip" link2="l_ankle" reason="Never" />
+    <disable_collisions link1="LHip" link2="neck" reason="Never" />
+    <disable_collisions link1="LHip" link2="r_ankle" 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="LShoulderDummy" reason="Never" />
+    <disable_collisions link1="LHipDummy" link2="LShoulder" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="RElbowDummy" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="RHipDummy" reason="Adjacent" />
+    <disable_collisions link1="LHipDummy" link2="RHip" reason="Never" />
+    <disable_collisions link1="LHipDummy" link2="RHipYaw" reason="Never" />
+    <disable_collisions link1="LHipDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LHipDummy" link2="gaze" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="l_sole" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="r_ankle" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="r_wrist" reason="Never" />
-    <disable_collisions link1="LHipDummy" link2="torso" reason="Adjacent" />
-    <disable_collisions link1="LKnee" link2="LShoulderDummy" reason="Never" />
+    <disable_collisions link1="LHipDummy" link2="neck" reason="Never" />
+    <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="RHip" reason="Never" />
+    <disable_collisions link1="LHipYaw" link2="RHipDummy" reason="Never" />
+    <disable_collisions link1="LHipYaw" link2="RHipYaw" reason="Adjacent" />
+    <disable_collisions link1="LHipYaw" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LHipYaw" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="LHipYaw" link2="gaze" reason="Never" />
+    <disable_collisions link1="LHipYaw" link2="neck" 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="RShoulder" reason="Never" />
     <disable_collisions link1="LKnee" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LKnee" link2="gaze" 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="l_sole" reason="Default" />
+    <disable_collisions link1="LKnee" link2="neck" reason="Never" />
     <disable_collisions link1="LKnee" link2="r_wrist" reason="Never" />
     <disable_collisions link1="LKnee" link2="torso" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="LShoulderDummy" reason="Adjacent" />
+    <disable_collisions link1="LShoulder" link2="RElbowDummy" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RHip" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RHipDummy" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RHipYaw" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RKnee" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RShoulderDummy" 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="r_ankle" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="r_sole" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="r_wrist" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="torso" reason="Adjacent" />
     <disable_collisions link1="LShoulderDummy" link2="RElbowDummy" 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" />
     <disable_collisions link1="LShoulderDummy" link2="RKnee" reason="Never" />
+    <disable_collisions link1="LShoulderDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RShoulderDummy" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="l_ankle" reason="Never" />
+    <disable_collisions link1="LShoulderDummy" link2="l_sole" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="l_wrist" reason="Never" />
+    <disable_collisions link1="LShoulderDummy" link2="neck" 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="Adjacent" />
+    <disable_collisions link1="LShoulderDummy" link2="torso" reason="Default" />
+    <disable_collisions link1="RElbowDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="RShoulderDummy" reason="Adjacent" />
     <disable_collisions link1="RElbowDummy" link2="l_ankle" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="l_sole" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="l_wrist" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="neck" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="r_ankle" reason="Never" />
     <disable_collisions link1="RElbowDummy" link2="r_wrist" reason="Adjacent" />
+    <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="l_ankle" reason="Never" />
+    <disable_collisions link1="RHip" link2="neck" 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="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="RHipDummy" link2="RShoulder" reason="Never" />
     <disable_collisions link1="RHipDummy" link2="gaze" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="l_ankle" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="l_wrist" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="r_ankle" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="r_sole" reason="Never" />
-    <disable_collisions link1="RHipDummy" link2="torso" reason="Adjacent" />
-    <disable_collisions link1="RKnee" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="RHipDummy" link2="neck" reason="Never" />
+    <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="neck" 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="l_wrist" reason="Never" />
+    <disable_collisions link1="RKnee" link2="neck" reason="Never" />
     <disable_collisions link1="RKnee" link2="r_ankle" reason="Adjacent" />
-    <disable_collisions link1="RKnee" link2="r_sole" reason="Never" />
+    <disable_collisions link1="RKnee" link2="r_sole" reason="Default" />
     <disable_collisions link1="RKnee" link2="torso" reason="Never" />
+    <disable_collisions link1="RShoulder" link2="RShoulderDummy" reason="Adjacent" />
+    <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="r_ankle" reason="Never" />
+    <disable_collisions link1="RShoulder" link2="r_sole" reason="Never" />
+    <disable_collisions link1="RShoulder" link2="r_wrist" reason="Never" />
+    <disable_collisions link1="RShoulder" link2="torso" reason="Adjacent" />
     <disable_collisions link1="RShoulderDummy" link2="l_ankle" reason="Never" />
     <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="r_ankle" reason="Never" />
+    <disable_collisions link1="RShoulderDummy" link2="r_sole" reason="Never" />
     <disable_collisions link1="RShoulderDummy" link2="r_wrist" reason="Never" />
-    <disable_collisions link1="RShoulderDummy" link2="torso" reason="Adjacent" />
+    <disable_collisions link1="RShoulderDummy" link2="torso" reason="Default" />
     <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="r_ankle" reason="Never" />
     <disable_collisions link1="gaze" link2="r_sole" reason="Never" />
-    <disable_collisions link1="gaze" link2="torso" reason="Adjacent" />
     <disable_collisions link1="l_ankle" link2="l_sole" reason="Adjacent" />
-    <disable_collisions link1="l_ankle" link2="l_wrist" reason="Never" />
+    <disable_collisions link1="l_ankle" link2="neck" reason="Never" />
     <disable_collisions link1="l_ankle" link2="r_wrist" reason="Never" />
     <disable_collisions link1="l_ankle" link2="torso" reason="Never" />
+    <disable_collisions link1="l_sole" link2="neck" reason="Never" />
     <disable_collisions link1="l_sole" link2="r_wrist" reason="Never" />
+    <disable_collisions link1="l_sole" link2="torso" reason="Never" />
+    <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="l_wrist" link2="r_wrist" reason="Default" />
+    <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="r_ankle" link2="r_sole" reason="Adjacent" />
     <disable_collisions link1="r_ankle" link2="torso" reason="Never" />
+    <disable_collisions link1="r_sole" link2="torso" reason="Never" />
 </robot>
diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H21.yaml b/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H21.yaml
index c34e518..e8adc1a 100644
--- a/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H21.yaml
+++ b/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H21.yaml
@@ -108,4 +108,19 @@ Pelvis:
     - RRTstarkConfigDefault
     - TRRTkConfigDefault
     - PRMkConfigDefault
-    - PRMstarkConfigDefault
\ No newline at end of file
+    - PRMstarkConfigDefault
+Body:
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+  projection_evaluator: joints(LHipYawPitch,LHipRoll)
+  longest_valid_segment_fraction: 0.05
\ No newline at end of file
diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H25.yaml b/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H25.yaml
index f6600e2..4086b31 100644
--- a/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H25.yaml
+++ b/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H25.yaml
@@ -138,4 +138,49 @@ LeftHand:
     - PRMkConfigDefault
     - PRMstarkConfigDefault
   projection_evaluator: joints(LWristYaw,LHand)
+  longest_valid_segment_fraction: 0.05
+RightArmHand:
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+  projection_evaluator: joints(RShoulderPitch,RShoulderRoll)
+  longest_valid_segment_fraction: 0.05
+LeftArmHand:
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+  projection_evaluator: joints(LShoulderPitch,LShoulderRoll)
+  longest_valid_segment_fraction: 0.05
+Body:
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+  projection_evaluator: joints(LHipYawPitch,LHipRoll)
   longest_valid_segment_fraction: 0.05
\ No newline at end of file
diff --git a/nao_dcm_apps/nao_dcm_moveit_config/package.xml b/nao_dcm_apps/nao_dcm_moveit_config/package.xml
index b5dbbd9..5704690 100644
--- a/nao_dcm_apps/nao_dcm_moveit_config/package.xml
+++ b/nao_dcm_apps/nao_dcm_moveit_config/package.xml
@@ -44,4 +44,4 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE
   <run_depend>xacro</run_depend>
   <run_depend>nao_dcm_description</run_depend>
   
-</package>
+</package>
\ No newline at end of file