From 125dc32da4a746a944a1c65d749ddd4da1361ef2 Mon Sep 17 00:00:00 2001
From: Konstantinos Chatzilygeroudis <costashatz@gmail.com>
Date: Fri, 30 May 2014 00:03:55 +0300
Subject: [PATCH] Added fingers to URDF and Gazebo..Integration with
 MimicJointPlugin for adding mimic joints to Gazebo

---
 README.md                                     |   3 +-
 nao_dcm_apps/nao_dcm_gazebo/CMakeLists.txt    |   2 +-
 nao_dcm_apps/nao_dcm_gazebo/package.xml       |   3 +-
 .../config/nao_robot_v4_H21.srdf              | 194 +++++++++++++++++-
 .../config/nao_robot_v4_H25.srdf              | 194 +++++++++++++++++-
 .../urdf/modules/hands.xacro                  | 128 ++++++++++++
 .../urdf/modules/robot.xacro                  |  46 +++++
 .../urdf/nao_dcm_H21.xacro                    |  35 +---
 .../urdf/nao_dcm_H25.xacro                    |  35 +---
 .../urdf/nao_dcm_gazebo.xacro                 | 164 +++++++++++++++
 .../urdf/nao_dcm_visuals_collisions.xacro     |  68 ++++++
 11 files changed, 799 insertions(+), 73 deletions(-)
 create mode 100644 nao_dcm_common/nao_dcm_description/urdf/modules/robot.xacro

diff --git a/README.md b/README.md
index b534aaf..4f7cebe 100644
--- a/README.md
+++ b/README.md
@@ -23,7 +23,7 @@ nao_dcm requires several packages to be installed in order to work properly:
 * [ROS MoveIt!] - Used for motion planning
 * [ROS Control] - **Version >=0.6.0**
 * [Webots for Nao] - The best simulator so far [optional]
-* [Gazebo] - Work in progress but with satisfactory results (**Version >= 2.2.2 alongside gazebo-ros-pkgs >= 2.3.4**) [optional]
+* [Gazebo] - Work in progress but with satisfactory results (**Version >= 2.2.2 alongside gazebo-ros-pkgs >= 2.3.4 and [gazebo_plugins]**) [optional]
 * [Nao Robot] - A real working Nao is the best "simulator" you'll ever get!! **Version >= V4.0 and flashed OpenNao OS >= 1.14.5**
 
 Basic Usage
@@ -92,4 +92,5 @@ Copyright (c) 2014, **Konstantinos Chatzilygeroudis**
 [nao robot]: http://www.aldebaran.com/en/humanoid-robot/nao-robot
 [nao_meshes]: https://github.com/vrabaud/nao_meshes
 [ros control]: http://wiki.ros.org/ros_control
+[gazebo_plugins]: http://www.github.com/costashatz/gazebo_plugins
     
\ No newline at end of file
diff --git a/nao_dcm_apps/nao_dcm_gazebo/CMakeLists.txt b/nao_dcm_apps/nao_dcm_gazebo/CMakeLists.txt
index 4dfc3c4..ffaf639 100644
--- a/nao_dcm_apps/nao_dcm_gazebo/CMakeLists.txt
+++ b/nao_dcm_apps/nao_dcm_gazebo/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(nao_dcm_gazebo)
 
 ## Find catkin
-find_package(catkin REQUIRED COMPONENTS gazebo_ros)
+find_package(catkin REQUIRED COMPONENTS gazebo_ros gazebo_plugins)
 
 ##Needed for ros packages
 catkin_package()
diff --git a/nao_dcm_apps/nao_dcm_gazebo/package.xml b/nao_dcm_apps/nao_dcm_gazebo/package.xml
index 4f9c2f6..8aefbf1 100644
--- a/nao_dcm_apps/nao_dcm_gazebo/package.xml
+++ b/nao_dcm_apps/nao_dcm_gazebo/package.xml
@@ -38,7 +38,8 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE
   <build_depend>gazebo_ros</build_depend>
 
   <run_depend>nao_dcm_description</run_depend>
-  <run_depend>gazebo_ros</run_depend> 
+  <run_depend>gazebo_ros</run_depend>
+  <run_depend>gazebo_plugins</run_depend>
 
 
   <export>
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 42923e1..c80d942 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,8 +71,16 @@
     <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="LFinger11" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="LFinger12" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="LFinger13" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="LFinger21" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="LFinger22" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="LFinger23" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="LShoulder" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="LShoulderDummy" reason="Adjacent" />
+    <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="RHip" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="RHipDummy" reason="Never" />
@@ -80,12 +88,77 @@
     <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="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="LElbowDummy" link2="torso" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="LFinger12" reason="Adjacent" />
+    <disable_collisions link1="LFinger11" link2="LFinger13" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="LFinger21" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="LFinger22" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="LFinger23" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="LShoulder" reason="Never" />
+    <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="l_wrist" reason="Adjacent" />
+    <disable_collisions link1="LFinger11" link2="neck" 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" />
+    <disable_collisions link1="LFinger12" link2="LFinger23" reason="Never" />
+    <disable_collisions link1="LFinger12" link2="LShoulder" reason="Never" />
+    <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="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="LFinger13" link2="LFinger21" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="LFinger22" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="LFinger23" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="LShoulder" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="LShoulderDummy" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="LThumb1" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="LThumb2" reason="Default" />
+    <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="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="RShoulder" reason="Never" />
+    <disable_collisions link1="LFinger21" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="LFinger21" link2="l_wrist" reason="Adjacent" />
+    <disable_collisions link1="LFinger21" link2="neck" reason="Never" />
+    <disable_collisions link1="LFinger21" link2="r_sole" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="LFinger23" reason="Adjacent" />
+    <disable_collisions link1="LFinger22" link2="LShoulder" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="LShoulderDummy" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="LThumb1" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="LThumb2" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="l_wrist" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="neck" reason="Never" />
+    <disable_collisions link1="LFinger23" link2="LShoulder" reason="Never" />
+    <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="l_wrist" reason="Never" />
+    <disable_collisions link1="LFinger23" link2="neck" 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" />
@@ -104,6 +177,7 @@
     <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="LHipDummy" link2="LHipYaw" reason="Default" />
     <disable_collisions link1="LHipDummy" link2="LKnee" reason="Adjacent" />
     <disable_collisions link1="LHipDummy" link2="LShoulder" reason="Never" />
@@ -123,12 +197,14 @@
     <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" />
     <disable_collisions link1="LHipYaw" link2="RKnee" reason="Never" />
     <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="gaze" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="helmet" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="l_ankle" reason="Never" />
@@ -149,13 +225,23 @@
     <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" />
     <disable_collisions link1="LShoulder" link2="RElbowDummy" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RFinger11" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RFinger12" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RFinger13" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RFinger21" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RFinger22" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RFinger23" 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="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" />
@@ -168,6 +254,8 @@
     <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="LThumb1" reason="Never" />
+    <disable_collisions link1="LShoulderDummy" link2="LThumb2" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RElbowDummy" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RHip" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RHipDummy" reason="Never" />
@@ -175,6 +263,8 @@
     <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="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_wrist" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="neck" reason="Never" />
@@ -184,6 +274,29 @@
     <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="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="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="RKnee" reason="Never" />
+    <disable_collisions link1="LThumb2" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LThumb2" link2="RShoulderDummy" 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" />
+    <disable_collisions link1="LThumb2" link2="r_sole" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="RFinger11" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="RFinger12" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="RFinger13" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="RFinger21" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="RFinger22" reason="Never" />
+    <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="l_ankle" reason="Never" />
@@ -192,6 +305,71 @@
     <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" />
+    <disable_collisions link1="RFinger11" link2="RFinger22" reason="Never" />
+    <disable_collisions link1="RFinger11" link2="RFinger23" reason="Never" />
+    <disable_collisions link1="RFinger11" link2="RShoulder" reason="Never" />
+    <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="neck" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="RFinger13" reason="Adjacent" />
+    <disable_collisions link1="RFinger12" link2="RFinger21" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="RFinger22" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="RFinger23" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="RShoulder" reason="Never" />
+    <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="neck" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="odroid" reason="Never" />
+    <disable_collisions link1="RFinger13" link2="RFinger21" reason="Never" />
+    <disable_collisions link1="RFinger13" link2="RFinger22" reason="Never" />
+    <disable_collisions link1="RFinger13" link2="RFinger23" reason="Never" />
+    <disable_collisions link1="RFinger13" link2="RShoulder" reason="Never" />
+    <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="neck" reason="Never" />
+    <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="neck" reason="Never" />
+    <disable_collisions link1="RFinger21" link2="l_sole" reason="Never" />
+    <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="neck" 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="neck" 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" />
@@ -215,6 +393,7 @@
     <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" />
@@ -236,6 +415,8 @@
     <disable_collisions link1="RKnee" link2="r_ankle" reason="Adjacent" />
     <disable_collisions link1="RKnee" link2="r_sole" reason="Adjacent" />
     <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" />
@@ -246,11 +427,22 @@
     <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" />
+    <disable_collisions link1="RShoulderDummy" link2="RThumb1" reason="Never" />
+    <disable_collisions link1="RShoulderDummy" link2="RThumb2" reason="Never" />
     <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="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" />
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 e1f9178..3487750 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,8 +102,16 @@
     <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="LFinger11" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="LFinger12" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="LFinger13" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="LFinger21" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="LFinger22" reason="Never" />
+    <disable_collisions link1="LElbowDummy" link2="LFinger23" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="LShoulder" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="LShoulderDummy" reason="Adjacent" />
+    <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="RHip" reason="Never" />
     <disable_collisions link1="LElbowDummy" link2="RHipDummy" reason="Never" />
@@ -111,12 +119,77 @@
     <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="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="LElbowDummy" link2="torso" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="LFinger12" reason="Adjacent" />
+    <disable_collisions link1="LFinger11" link2="LFinger13" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="LFinger21" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="LFinger22" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="LFinger23" reason="Never" />
+    <disable_collisions link1="LFinger11" link2="LShoulder" reason="Never" />
+    <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="l_wrist" reason="Adjacent" />
+    <disable_collisions link1="LFinger11" link2="neck" 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" />
+    <disable_collisions link1="LFinger12" link2="LFinger23" reason="Never" />
+    <disable_collisions link1="LFinger12" link2="LShoulder" reason="Never" />
+    <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="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="LFinger13" link2="LFinger21" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="LFinger22" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="LFinger23" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="LShoulder" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="LShoulderDummy" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="LThumb1" reason="Never" />
+    <disable_collisions link1="LFinger13" link2="LThumb2" reason="Default" />
+    <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="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="RShoulder" reason="Never" />
+    <disable_collisions link1="LFinger21" link2="RShoulderDummy" reason="Never" />
+    <disable_collisions link1="LFinger21" link2="l_wrist" reason="Adjacent" />
+    <disable_collisions link1="LFinger21" link2="neck" reason="Never" />
+    <disable_collisions link1="LFinger21" link2="r_sole" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="LFinger23" reason="Adjacent" />
+    <disable_collisions link1="LFinger22" link2="LShoulder" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="LShoulderDummy" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="LThumb1" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="LThumb2" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="l_wrist" reason="Never" />
+    <disable_collisions link1="LFinger22" link2="neck" reason="Never" />
+    <disable_collisions link1="LFinger23" link2="LShoulder" reason="Never" />
+    <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="l_wrist" reason="Never" />
+    <disable_collisions link1="LFinger23" link2="neck" 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" />
@@ -135,6 +208,7 @@
     <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="LHipDummy" link2="LHipYaw" reason="Default" />
     <disable_collisions link1="LHipDummy" link2="LKnee" reason="Adjacent" />
     <disable_collisions link1="LHipDummy" link2="LShoulder" reason="Never" />
@@ -154,12 +228,14 @@
     <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" />
     <disable_collisions link1="LHipYaw" link2="RKnee" reason="Never" />
     <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="gaze" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="helmet" reason="Never" />
     <disable_collisions link1="LHipYaw" link2="l_ankle" reason="Never" />
@@ -180,13 +256,23 @@
     <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" />
     <disable_collisions link1="LShoulder" link2="RElbowDummy" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RFinger11" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RFinger12" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RFinger13" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RFinger21" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RFinger22" reason="Never" />
+    <disable_collisions link1="LShoulder" link2="RFinger23" 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="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" />
@@ -199,6 +285,8 @@
     <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="LThumb1" reason="Never" />
+    <disable_collisions link1="LShoulderDummy" link2="LThumb2" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RElbowDummy" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RHip" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="RHipDummy" reason="Never" />
@@ -206,6 +294,8 @@
     <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="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_wrist" reason="Never" />
     <disable_collisions link1="LShoulderDummy" link2="neck" reason="Never" />
@@ -215,6 +305,29 @@
     <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="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="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="RKnee" reason="Never" />
+    <disable_collisions link1="LThumb2" link2="RShoulder" reason="Never" />
+    <disable_collisions link1="LThumb2" link2="RShoulderDummy" 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" />
+    <disable_collisions link1="LThumb2" link2="r_sole" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="RFinger11" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="RFinger12" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="RFinger13" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="RFinger21" reason="Never" />
+    <disable_collisions link1="RElbowDummy" link2="RFinger22" reason="Never" />
+    <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="l_ankle" reason="Never" />
@@ -223,6 +336,71 @@
     <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" />
+    <disable_collisions link1="RFinger11" link2="RFinger22" reason="Never" />
+    <disable_collisions link1="RFinger11" link2="RFinger23" reason="Never" />
+    <disable_collisions link1="RFinger11" link2="RShoulder" reason="Never" />
+    <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="neck" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="RFinger13" reason="Adjacent" />
+    <disable_collisions link1="RFinger12" link2="RFinger21" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="RFinger22" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="RFinger23" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="RShoulder" reason="Never" />
+    <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="neck" reason="Never" />
+    <disable_collisions link1="RFinger12" link2="odroid" reason="Never" />
+    <disable_collisions link1="RFinger13" link2="RFinger21" reason="Never" />
+    <disable_collisions link1="RFinger13" link2="RFinger22" reason="Never" />
+    <disable_collisions link1="RFinger13" link2="RFinger23" reason="Never" />
+    <disable_collisions link1="RFinger13" link2="RShoulder" reason="Never" />
+    <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="neck" reason="Never" />
+    <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="neck" reason="Never" />
+    <disable_collisions link1="RFinger21" link2="l_sole" reason="Never" />
+    <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="neck" 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="neck" 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" />
@@ -246,6 +424,7 @@
     <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" />
@@ -267,6 +446,8 @@
     <disable_collisions link1="RKnee" link2="r_ankle" reason="Adjacent" />
     <disable_collisions link1="RKnee" link2="r_sole" reason="Adjacent" />
     <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" />
@@ -277,11 +458,22 @@
     <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" />
+    <disable_collisions link1="RShoulderDummy" link2="RThumb1" reason="Never" />
+    <disable_collisions link1="RShoulderDummy" link2="RThumb2" reason="Never" />
     <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="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" />
diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/hands.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/hands.xacro
index 2de1db3..fd965a8 100644
--- a/nao_dcm_common/nao_dcm_description/urdf/modules/hands.xacro
+++ b/nao_dcm_common/nao_dcm_description/urdf/modules/hands.xacro
@@ -1,6 +1,104 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
+	<!-- Fingers -->
+	<xacro:macro name="Finger" params="side num side_small reflect">
+		<joint name="${side}Finger${num}" type="revolute">
+	    	<parent link="${side_small}_wrist"/>
+	    	<child link="${side}Finger${num}1"/>
+	    	<origin xyz="0.06907 ${reflect*0.01157} -0.00304" rpy="0 0 0"/>
+	    	<axis xyz="0 1 0"/>
+	    	<mimic joint="${side}Hand" multiplier="-${PI/4}" offset="${PI/4}"/>
+	    	<!-- Dummy values for mimic joint -->
+	    	<limit effort="10.0" lower="0.0" upper="${PI/4}" velocity="3.0"/>
+	    </joint>
+	    <link name="${side}Finger${num}1">
+	    	<xacro:visuals_collisions_phalax reflect="-1" side="${side}"/>
+	    	<inertial> <!-- dummy values for gazebo -->
+	            <mass value="0.2e-5" />
+	            <origin xyz="0 0 0"/>
+	            <inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" />
+	        </inertial>
+	    </link>
+	    <joint name="${side}Finger${num}_1" type="revolute">
+	    	<parent link="${side}Finger${num}1"/>
+	    	<child link="${side}Finger${num}2"/>
+	    	<origin xyz="0.01436 0 0" rpy="0 0 0"/>
+	    	<axis xyz="0 1 0"/>
+	    	<mimic joint="${side}Hand" multiplier="-${PI/4}" offset="${PI/4}"/>
+	    	<!-- Dummy values for mimic joint -->
+	    	<limit effort="10.0" lower="0.0" upper="${PI/4}" velocity="3.0"/>
+	    </joint>
+	    <link name="${side}Finger${num}2">
+	    	<xacro:visuals_collisions_phalax reflect="-1" side="${side}"/>
+	    	<inertial> <!-- dummy values for gazebo -->
+	            <mass value="0.2e-5" />
+	            <origin xyz="0 0 0"/>
+	            <inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" />
+	        </inertial>
+	    </link>
+	    <joint name="${side}Finger${num}_2" type="revolute">
+	    	<parent link="${side}Finger${num}2"/>
+	    	<child link="${side}Finger${num}3"/>
+	    	<origin xyz="0.01436 0 0" rpy="0 0 0"/>
+	    	<axis xyz="0 1 0"/>
+	    	<mimic joint="${side}Hand" multiplier="-${PI/4}" offset="${PI/4}"/>
+	    	<!-- Dummy values for mimic joint -->
+	    	<limit effort="10.0" lower="0.0" upper="${PI/4}" velocity="3.0"/>
+	    </joint>
+	    <link name="${side}Finger${num}3">
+	    	<xacro:visuals_collisions_phalax_end reflect="-1" side="${side}"/>
+	    	<inertial> <!-- dummy values for gazebo -->
+	            <mass value="0.2e-5" />
+	            <origin xyz="0 0 0"/>
+	            <inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" />
+	        </inertial>
+	    </link>
+	</xacro:macro>
+	<xacro:Finger side="L" side_small="l" num="1" reflect="1"/>
+	<xacro:Finger side="L" side_small="l" num="2" reflect="-1"/>
+	<xacro:Finger side="R" side_small="r" num="1" reflect="1"/>
+	<xacro:Finger side="R" side_small="r" num="2" reflect="-1"/>
+	<!-- Thumbs -->
+	<xacro:macro name="Thumb" params="side side_small">
+		<joint name="${side}Thumb1_joint" type="revolute">
+	    	<parent link="${side_small}_wrist"/>
+	    	<child link="${side}Thumb1"/>
+	    	<origin xyz="0.04895 0 -0.02638" rpy="${PI} ${PI/4} 0"/>
+	    	<axis xyz="0 1 0"/>
+	    	<mimic joint="${side}Hand" multiplier="-${PI/4}" offset="${PI/4}"/>
+	    	<!-- Dummy values for mimic joint -->
+	    	<limit effort="10.0" lower="0.0" upper="${PI/4}" velocity="3.0"/>
+	    </joint>
+	    <link name="${side}Thumb1">
+	    	<xacro:visuals_collisions_phalax reflect="-1" side="${side}"/>
+	    	<inertial> <!-- dummy values for gazebo -->
+	            <mass value="0.2e-5" />
+	            <origin xyz="0 0 0"/>
+	            <inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" />
+	        </inertial>
+	    </link>
+	    <joint name="${side}Thumb1_2" type="revolute">
+	    	<parent link="${side}Thumb1"/>
+	    	<child link="${side}Thumb2"/>
+	    	<origin xyz="0.01436 0 0" rpy="0 0 0"/>
+	    	<axis xyz="0 1 0"/>
+	    	<mimic joint="${side}Hand" multiplier="-${PI/4}" offset="${PI/4}"/>
+	    	<!-- Dummy values for mimic joint -->
+	    	<limit effort="10.0" lower="0.0" upper="${PI/4}" velocity="3.0"/>
+	    </joint>
+	    <link name="${side}Thumb2">
+	    	<xacro:visuals_collisions_phalax_end reflect="-1" side="${side}"/>
+	    	<inertial> <!-- dummy values for gazebo -->
+	            <mass value="0.2e-5" />
+	            <origin xyz="0 0 0"/>
+	            <inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" />
+	        </inertial>
+	    </link>
+	</xacro:macro>
+	<xacro:Thumb side="L" side_small="l"/>
+	<xacro:Thumb side="R" side_small="r"/>
+
 	<!-- H25 Hands -->
 	<xacro:if value="${H25}">
 	    <joint name="LWristYaw" type="revolute">
@@ -93,5 +191,35 @@
 	            <inertia ixx="0.00007810423" ixy="-0.00001233383" ixz="0.00000744169" iyy="0.00112084378" iyz="-0.00000054551" izz="0.00112214306"/>
 	        </inertial>
 	    </link>
+
+	    <!-- DUMMY REVOLUTE JOINT FOR NON MOTORED HAND -->
+	    <joint name="LHand" type="revolute">
+	        <parent link="l_wrist"/>
+	        <child link="l_gripper"/>
+	        <origin rpy="0 0 0" xyz="0.05775 0 0"/>
+	        <limit effort="10.0" lower="0.0" upper="0.0" velocity="3.0"/>
+	    </joint>
+	    <link name="l_gripper">
+	        <inertial> <!-- dummy values for gazebo -->
+	            <mass value="0.2e-5" />
+	            <origin xyz="0 0 0"/>
+	            <inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" />
+	        </inertial>
+	    </link>
+
+	    <!-- DUMMY REVOLUTE JOINT FOR NON MOTORED HAND -->
+	    <joint name="RHand" type="revolute">
+	        <parent link="r_wrist"/>
+	        <child link="r_gripper"/>
+	        <origin rpy="0 0 0" xyz="0.05775 0 0"/>
+	        <limit effort="10.0" lower="0.0" upper="0.0" velocity="3.0"/>
+	    </joint>
+	    <link name="r_gripper">
+	        <inertial> <!-- dummy values for gazebo -->
+	            <mass value="0.2e-5" />
+	            <origin xyz="0 0 0"/>
+	            <inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" />
+	        </inertial>
+	    </link>
 	</xacro:unless>
 </robot>
\ No newline at end of file
diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/robot.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/robot.xacro
new file mode 100644
index 0000000..d3fe303
--- /dev/null
+++ b/nao_dcm_common/nao_dcm_description/urdf/modules/robot.xacro
@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+	<!-- XACRO PROPERTIES -->
+	<xacro:property name="PI" value="3.14159265359" />
+	<xacro:property name="PI_2" value="1.57079632679" />
+	<xacro:property name="scale" value="0.001" />
+	<xacro:property name="nao_meshes" value="true" />
+	<xacro:property name="use_helmet" value="true" />
+	<xacro:property name="helmet_cameras_viz" value="false" />
+	<xacro:property name="use_odroid" value="true" />
+	<xacro:property name="use_hector_imu" value="false" />
+
+	<!-- Which IMU plugin to use (hector or default) -->
+	<xacro:if value="${use_hector_imu}">
+		<xacro:property name="imu_plugin" value="libhector_gazebo_ros_imu.so" />
+	</xacro:if>
+	<xacro:unless value="${use_hector_imu}">
+		<xacro:property name="imu_plugin" value="libgazebo_ros_imu.so" />
+	</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" />
+
+	<!-- Load xacro modules/parts of the robot -->
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/torso.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/head.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/arms.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/hands.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/legs.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/imu.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/sonars.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/cameras.xacro" />
+
+	<!-- Whether to use Nao helmet or not -->
+	<xacro:if value="${use_helmet}">
+		<xacro:include filename="$(find nao_dcm_description)/urdf/modules/helmet.xacro" />
+		<xacro:include filename="$(find nao_dcm_description)/urdf/modules/helmet_cameras.xacro" />
+	</xacro:if>
+
+	<!-- Whether to use Odroid or not -->
+	<xacro:if value="${use_odroid}">
+		<xacro:include filename="$(find nao_dcm_description)/urdf/modules/odroid_bag.xacro" />
+	</xacro:if>
+
+</robot>
\ No newline at end of file
diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21.xacro
index aae24f1..fe2496c 100644
--- a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21.xacro
+++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21.xacro
@@ -2,43 +2,10 @@
 <robot name="nao_robot_v4_H21" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
 	<!-- XACRO PROPERTIES -->
-	<xacro:property name="PI" value="3.14159265359" />
-	<xacro:property name="PI_2" value="1.57079632679" />
-	<xacro:property name="scale" value="0.001" />
 	<xacro:property name="H25" value="false" />
-	<xacro:property name="nao_meshes" value="true" />
-	<xacro:property name="use_helmet" value="true" />
-	<xacro:property name="helmet_cameras_viz" value="false" />
-	<xacro:property name="use_odroid" value="true" />
-	<xacro:property name="use_hector_imu" value="false" />
 
-	<xacro:if value="${use_hector_imu}">
-		<xacro:property name="imu_plugin" value="libhector_gazebo_ros_imu.so" />
-	</xacro:if>
-	<xacro:unless value="${use_hector_imu}">
-		<xacro:property name="imu_plugin" value="libgazebo_ros_imu.so" />
-	</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:include filename="$(find nao_dcm_description)/urdf/nao_dcm_H21_control.xacro" />
 
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/torso.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/head.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/arms.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/hands.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/legs.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/imu.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/sonars.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/cameras.xacro" />
-
-	<xacro:if value="${use_helmet}">
-		<xacro:include filename="$(find nao_dcm_description)/urdf/modules/helmet.xacro" />
-		<xacro:include filename="$(find nao_dcm_description)/urdf/modules/helmet_cameras.xacro" />
-	</xacro:if>
-
-	<xacro:if value="${use_odroid}">
-		<xacro:include filename="$(find nao_dcm_description)/urdf/modules/odroid_bag.xacro" />
-	</xacro:if>
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/robot.xacro" />
 
 </robot>
\ No newline at end of file
diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25.xacro
index 2dc252e..e153a72 100644
--- a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25.xacro
+++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25.xacro
@@ -2,43 +2,10 @@
 <robot name="nao_robot_v4_H25" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
 	<!-- XACRO PROPERTIES -->
-	<xacro:property name="PI" value="3.14159265359" />
-	<xacro:property name="PI_2" value="1.57079632679" />
-	<xacro:property name="scale" value="0.001" />
 	<xacro:property name="H25" value="true" />
-	<xacro:property name="nao_meshes" value="true" />
-	<xacro:property name="use_helmet" value="true" />
-	<xacro:property name="helmet_cameras_viz" value="false" />
-	<xacro:property name="use_odroid" value="true" />
-	<xacro:property name="use_hector_imu" value="false" />
 
-	<xacro:if value="${use_hector_imu}">
-		<xacro:property name="imu_plugin" value="libhector_gazebo_ros_imu.so" />
-	</xacro:if>
-	<xacro:unless value="${use_hector_imu}">
-		<xacro:property name="imu_plugin" value="libgazebo_ros_imu.so" />
-	</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:include filename="$(find nao_dcm_description)/urdf/nao_dcm_H25_control.xacro" />
 
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/torso.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/head.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/arms.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/hands.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/legs.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/imu.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/sonars.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/cameras.xacro" />
-
-	<xacro:if value="${use_helmet}">
-		<xacro:include filename="$(find nao_dcm_description)/urdf/modules/helmet.xacro" />
-		<xacro:include filename="$(find nao_dcm_description)/urdf/modules/helmet_cameras.xacro" />
-	</xacro:if>
-
-	<xacro:if value="${use_odroid}">
-		<xacro:include filename="$(find nao_dcm_description)/urdf/modules/odroid_bag.xacro" />
-	</xacro:if>
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/robot.xacro" />
 
 </robot>
\ No newline at end of file
diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_gazebo.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_gazebo.xacro
index 5805e2a..ffadaa8 100644
--- a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_gazebo.xacro
+++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_gazebo.xacro
@@ -1,5 +1,42 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <!-- MACROS FOR MIMIC JOINTS -->
+    <!-- Fingers -->
+    <xacro:macro name="FingerMimics" params="side num">
+        <plugin name="mimic_${side}Finger${num}" filename="libgazebo_mimic_joint_plugin.so">
+            <joint>${side}Hand</joint>
+            <mimicJoint>${side}Finger${num}</mimicJoint>
+            <multiplier>-${PI/4}</multiplier>
+            <offset>${PI/4}</offset>
+        </plugin>
+        <plugin name="mimic_${side}Finger${num}_1" filename="libgazebo_mimic_joint_plugin.so">
+            <joint>${side}Hand</joint>
+            <mimicJoint>${side}Finger${num}_1</mimicJoint>
+            <multiplier>-${PI/4}</multiplier>
+            <offset>${PI/4}</offset>
+        </plugin>
+        <plugin name="mimic_${side}Finger${num}_2" filename="libgazebo_mimic_joint_plugin.so">
+            <joint>${side}Hand</joint>
+            <mimicJoint>${side}Finger${num}_2</mimicJoint>
+            <multiplier>-${PI/4}</multiplier>
+            <offset>${PI/4}</offset>
+        </plugin>
+    </xacro:macro>
+    <!-- Thumbs -->
+    <xacro:macro name="ThumbMimics" params="side">
+        <plugin name="${side}Thumb1_joint" filename="libgazebo_mimic_joint_plugin.so">
+            <joint>${side}Hand</joint>
+            <mimicJoint>${side}Thumb1_joint</mimicJoint>
+            <multiplier>-${PI/4}</multiplier>
+            <offset>${PI/4}</offset>
+        </plugin>
+        <plugin name="${side}Thumb1_2" filename="libgazebo_mimic_joint_plugin.so">
+            <joint>${side}Hand</joint>
+            <mimicJoint>${side}Thumb1_2</mimicJoint>
+            <multiplier>-${PI/4}</multiplier>
+            <offset>${PI/4}</offset>
+        </plugin>
+    </xacro:macro>
     <!-- GAZEBO ROS CONTROL AND IMU PLUGIN -->
     <gazebo>
         <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
@@ -20,6 +57,21 @@
             <updateRate>30.0</updateRate>
             <alwaysOn>true</alwaysOn>
         </plugin>
+        <!-- Mimic joints through plugin develop by costashatz based on implementation by Goncalo Cabrita -->
+        <plugin name="mimic_hipyawpitch" filename="libgazebo_mimic_joint_plugin.so">
+            <joint>LHipYawPitch</joint>
+            <mimicJoint>RHipYawPitch</mimicJoint>
+            <multiplier>1.0</multiplier>
+            <offset>0.0</offset>
+        </plugin>
+
+        <xacro:FingerMimics side="L" num="1"/>
+        <xacro:FingerMimics side="L" num="2"/>
+        <xacro:FingerMimics side="R" num="1"/>
+        <xacro:FingerMimics side="R" num="2"/>
+
+        <xacro:ThumbMimics side="L"/>
+        <xacro:ThumbMimics side="R"/>
     </gazebo>
     <!-- END OF GAZEBO ROS CONTROL PLUGIN -->
     <!-- GAZEBO MATERIALS -->
@@ -84,6 +136,62 @@
         <selfCollide>false</selfCollide>
         <turnGravityOff>false</turnGravityOff>
     </gazebo>
+    <gazebo reference="LFinger11">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="LFinger12">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="LFinger13">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="LFinger21">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="LFinger22">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="LFinger23">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="LThumb1">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="LThumb2">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
     <gazebo reference="l_gripper">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
@@ -117,6 +225,62 @@
         <selfCollide>false</selfCollide>
         <turnGravityOff>false</turnGravityOff>
     </gazebo>
+    <gazebo reference="RFinger11">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="RFinger12">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="RFinger13">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="RFinger21">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="RFinger22">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="RFinger23">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="RThumb1">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+    <gazebo reference="RThumb2">
+        <material>Gazebo/Yellow</material>
+        <mu1>0.5</mu1>
+        <mu2>0.5</mu2>
+        <selfCollide>false</selfCollide>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
     <gazebo reference="r_gripper">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
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 dc02b1f..d6c4871 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
@@ -252,6 +252,74 @@
 		</xacro:if>
     </xacro:macro>
 
+    <xacro:macro name="visuals_collisions_phalax" params="side reflect">
+    	<xacro:unless value="${nao_meshes}">
+			<visual>
+				<origin xyz="0.000718 0 0" rpy="${PI_2} 0 ${PI_2}"/>
+	            <geometry>
+	                <box size="0.01 0.01 0.01436"/>
+	            </geometry>
+				<material name="Yellow"/>
+			</visual>
+			<collision>
+				<origin xyz="0.000718 0 0" rpy="${PI_2} 0 ${PI_2}"/>
+	            <geometry>
+	                <box size="0.01 0.01 0.01436"/>
+	            </geometry>
+			</collision>
+		</xacro:unless>
+		<xacro:if value="${nao_meshes}">
+			<visual>
+				<origin xyz="0 0 0" rpy="-${PI_2} -0 0"/>
+				<geometry>
+					<mesh filename="package://nao_meshes/meshes/V40/visual/Phalax.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_meshes/meshes/V40/collision/Phalax.dae"  scale="${scale} ${scale} ${scale}"/>
+				</geometry>
+				<material name="LightGrey"/>
+			</collision>
+		</xacro:if>
+    </xacro:macro>
+
+    <xacro:macro name="visuals_collisions_phalax_end" params="side reflect">
+    	<xacro:unless value="${nao_meshes}">
+			<visual>
+				<origin xyz="0.000718 0 0" rpy="${PI_2} 0 ${PI_2}"/>
+	            <geometry>
+	                <box size="0.01 0.01 0.01436"/>
+	            </geometry>
+				<material name="Yellow"/>
+			</visual>
+			<collision>
+				<origin xyz="0.000718 0 0" rpy="${PI_2} 0 ${PI_2}"/>
+	            <geometry>
+	                <box size="0.01 0.01 0.01436"/>
+	            </geometry>
+			</collision>
+		</xacro:unless>
+		<xacro:if value="${nao_meshes}">
+			<visual>
+				<origin xyz="0 0 0" rpy="-${PI_2} -0 0"/>
+				<geometry>
+					<mesh filename="package://nao_meshes/meshes/V40/visual/PhalaxEnd.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_meshes/meshes/V40/collision/PhalaxEnd.dae"  scale="${scale} ${scale} ${scale}"/>
+				</geometry>
+				<material name="LightGrey"/>
+			</collision>
+		</xacro:if>
+    </xacro:macro>
+
     <xacro:macro name="visuals_collisions_hip_yaw_pitch" params="side reflect">
     	<xacro:if value="${nao_meshes}">
 			<visual>