diff --git a/robots/simple_humanoid_description/meshes/box.stl b/robots/simple_humanoid_description/meshes/box.stl
new file mode 100644
index 0000000000000000000000000000000000000000..6e11e77274f3caae65deb988572e62353882469d
--- /dev/null
+++ b/robots/simple_humanoid_description/meshes/box.stl
@@ -0,0 +1,86 @@
+solid Exported from Blender-2.78 (sub 0)
+facet normal -1.000000 0.000000 0.000000
+outer loop
+vertex -0.500000 -0.500000 -0.500000
+vertex -0.500000 -0.500000 0.500000
+vertex -0.500000 0.500000 0.500000
+endloop
+endfacet
+facet normal -1.000000 0.000000 0.000000
+outer loop
+vertex -0.500000 0.500000 0.500000
+vertex -0.500000 0.500000 -0.500000
+vertex -0.500000 -0.500000 -0.500000
+endloop
+endfacet
+facet normal 0.000000 1.000000 0.000000
+outer loop
+vertex -0.500000 0.500000 -0.500000
+vertex -0.500000 0.500000 0.500000
+vertex 0.500000 0.500000 0.500000
+endloop
+endfacet
+facet normal 0.000000 1.000000 0.000000
+outer loop
+vertex 0.500000 0.500000 0.500000
+vertex 0.500000 0.500000 -0.500000
+vertex -0.500000 0.500000 -0.500000
+endloop
+endfacet
+facet normal 1.000000 0.000000 0.000000
+outer loop
+vertex 0.500000 0.500000 -0.500000
+vertex 0.500000 0.500000 0.500000
+vertex 0.500000 -0.500000 0.500000
+endloop
+endfacet
+facet normal 1.000000 0.000000 0.000000
+outer loop
+vertex 0.500000 -0.500000 0.500000
+vertex 0.500000 -0.500000 -0.500000
+vertex 0.500000 0.500000 -0.500000
+endloop
+endfacet
+facet normal 0.000000 -1.000000 0.000000
+outer loop
+vertex -0.500000 -0.500000 0.500000
+vertex -0.500000 -0.500000 -0.500000
+vertex 0.500000 -0.500000 -0.500000
+endloop
+endfacet
+facet normal 0.000000 -1.000000 0.000000
+outer loop
+vertex 0.500000 -0.500000 -0.500000
+vertex 0.500000 -0.500000 0.500000
+vertex -0.500000 -0.500000 0.500000
+endloop
+endfacet
+facet normal 0.000000 0.000000 -1.000000
+outer loop
+vertex 0.500000 -0.500000 -0.500000
+vertex -0.500000 -0.500000 -0.500000
+vertex -0.500000 0.500000 -0.500000
+endloop
+endfacet
+facet normal 0.000000 0.000000 -1.000000
+outer loop
+vertex -0.500000 0.500000 -0.500000
+vertex 0.500000 0.500000 -0.500000
+vertex 0.500000 -0.500000 -0.500000
+endloop
+endfacet
+facet normal 0.000000 0.000000 1.000000
+outer loop
+vertex 0.500000 0.500000 0.500000
+vertex -0.500000 0.500000 0.500000
+vertex -0.500000 -0.500000 0.500000
+endloop
+endfacet
+facet normal 0.000000 0.000000 1.000000
+outer loop
+vertex -0.500000 -0.500000 0.500000
+vertex 0.500000 -0.500000 0.500000
+vertex 0.500000 0.500000 0.500000
+endloop
+endfacet
+endsolid Exported from Blender-2.78 (sub 0)
diff --git a/robots/simple_humanoid_description/srdf/simple_humanoid.srdf b/robots/simple_humanoid_description/srdf/simple_humanoid.srdf
index 6ef70c255318b8d58c68566fda974d3d3cb4d0ec..72c92468f898275e2ee6ddc78913d6100bcbd775 100644
--- a/robots/simple_humanoid_description/srdf/simple_humanoid.srdf
+++ b/robots/simple_humanoid_description/srdf/simple_humanoid.srdf
@@ -128,6 +128,38 @@
     <joint name="CHEST"           value="0" />
   </group_state>
 
+    <rotor_params>
+        <joint name="WAIST_P" mass="1.0" gear_ratio="0.0" />
+        <joint name="WAIST_R" mass="0.0" gear_ratio="1.0" />
+        <joint name="CHEST" mass="0.0" gear_ratio="0.0" />
+        <joint name="LARM_SHOULDER_P" mass="0.0" gear_ratio="0.0" />
+        <joint name="LARM_SHOULDER_R" mass="0.0" gear_ratio="0.0" />
+        <joint name="LARM_SHOULDER_Y" mass="0.0" gear_ratio="0.0" />
+        <joint name="LARM_ELBOW" mass="0.0" gear_ratio="0.0" />
+        <joint name="LARM_WRIST_Y" mass="0.0" gear_ratio="0.0" />
+        <joint name="LARM_WRIST_P" mass="0.0" gear_ratio="0.0" />
+        <joint name="LARM_WRIST_R" mass="0.0" gear_ratio="0.0" />
+        <joint name="RARM_SHOULDER_P" mass="0.0" gear_ratio="0.0" />
+        <joint name="RARM_SHOULDER_R" mass="0.0" gear_ratio="0.0" />
+        <joint name="RARM_SHOULDER_Y" mass="0.0" gear_ratio="0.0" />
+        <joint name="RARM_ELBOW" mass="0.0" gear_ratio="0.0" />
+        <joint name="RARM_WRIST_Y" mass="0.0" gear_ratio="0.0" />
+        <joint name="RARM_WRIST_P" mass="0.0" gear_ratio="0.0" />
+        <joint name="RARM_WRIST_R" mass="0.0" gear_ratio="0.0" />
+        <joint name="LLEG_HIP_R" mass="0.0" gear_ratio="0.0" />
+        <joint name="LLEG_HIP_P" mass="0.0" gear_ratio="0.0" />
+        <joint name="LLEG_HIP_Y" mass="0.0" gear_ratio="0.0" />
+        <joint name="LLEG_KNEE" mass="0.0" gear_ratio="0.0" />
+        <joint name="LLEG_ANKLE_P" mass="0.0" gear_ratio="0.0" />
+        <joint name="LLEG_ANKLE_R" mass="0.0" gear_ratio="0.0" />
+        <joint name="RLEG_HIP_R" mass="0.0" gear_ratio="0.0" />
+        <joint name="RLEG_HIP_P" mass="0.0" gear_ratio="0.0" />
+        <joint name="RLEG_HIP_Y" mass="0.0" gear_ratio="0.0" />
+        <joint name="RLEG_KNEE" mass="0.0" gear_ratio="0.0" />
+        <joint name="RLEG_ANKLE_P" mass="0.0" gear_ratio="0.0" />
+        <joint name="RLEG_ANKLE_R" mass="0.0" gear_ratio="0.0" />
+    </rotor_params>
+
   <!--
    Simple Humnaoid Specificities.
      foot height = y axis
diff --git a/robots/simple_humanoid_description/urdf/simple_humanoid.urdf b/robots/simple_humanoid_description/urdf/simple_humanoid.urdf
index 7458fbc64c6e49d520b76a0d694b56647451f2c1..5c0530b3dd3affc8f64d80d6232e5ead41b12a0f 100644
--- a/robots/simple_humanoid_description/urdf/simple_humanoid.urdf
+++ b/robots/simple_humanoid_description/urdf/simple_humanoid.urdf
@@ -14,6 +14,22 @@
       <mass value="27"/>
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
     </inertial>
+    <collision name="test">
+      <geometry>
+        <cylinder radius="1" length="1"/>
+      </geometry>
+    </collision>
+    <collision name="box">
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/simple_humanoid_description/meshes/box.stl" />
+      </geometry>
+    </collision>
+    <collision_checking>
+      <!--- This tells to pinocchio to replace the cylinder called "test"
+           by a capsule with the same radius and length -->
+      <capsule name="test"/>
+      <convex name="box"/>
+    </collision_checking>
   </link>
 
   <link name="WAIST_LINK1">