diff --git a/robots/anymal_b_simple_description/robots/anymal-kinova.urdf b/robots/anymal_b_simple_description/robots/anymal-kinova.urdf
index 6e019390c5705186ec38bec16bf2e1c856f6f321..a807f0531cb0e3551b96253c33308a78504fba68 100644
--- a/robots/anymal_b_simple_description/robots/anymal-kinova.urdf
+++ b/robots/anymal_b_simple_description/robots/anymal-kinova.urdf
@@ -101,25 +101,24 @@
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="-0.001960558279 -0.001413217745 0.050207125344"/>
-      <mass value="16.793507758"/>
-      <inertia ixx="0.217391101503" ixy="-0.00132873239126" ixz="-0.00228200226173" iyy="0.639432546734" iyz="-0.00138078263145" izz="0.62414077654"/>
+      <mass value="1e-6"/>
+      <inertia ixx="1e-6" ixy="1e-6" ixz="1e-6" iyy="1e-6" iyz="1e-6" izz="1e-6"/>
     </inertial>
   </link>
   <!-- Fixed joint to add dummy inertia link -->
-  <!-- <joint name="base_to_base_inertia" type="fixed">
+  <joint name="base_to_base_inertia" type="fixed">
     <parent link="base"/>
     <child link="base_inertia"/>
     <origin rpy="0 0 0" xyz="0 0 0"/>
-  </joint> -->
+  </joint>
   <!-- Dummy inertia link, because KDL cannot have inertia on the base link -->
-  <!-- <link name="base_inertia">
+  <link name="base_inertia">
     <inertial>
       <origin rpy="0 0 0" xyz="-0.001960558279 -0.001413217745 0.050207125344"/>
       <mass value="16.793507758"/>
       <inertia ixx="0.217391101503" ixy="-0.00132873239126" ixz="-0.00228200226173" iyy="0.639432546734" iyz="-0.00138078263145" izz="0.62414077654"/>
     </inertial>
-  </link> -->
+  </link>
   <!-- Xacro:Properties -->
   <!-- [kg * m^2] -->
   <!-- [A] -->
diff --git a/robots/anymal_b_simple_description/robots/anymal.urdf b/robots/anymal_b_simple_description/robots/anymal.urdf
index 1b2c190ec14011f66ce139e03c5414d610c6be2f..0613b002ca986e4fcd626a49ea62ca51b5be0312 100644
--- a/robots/anymal_b_simple_description/robots/anymal.urdf
+++ b/robots/anymal_b_simple_description/robots/anymal.urdf
@@ -101,25 +101,24 @@
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="-0.001960558279 -0.001413217745 0.050207125344"/>
-      <mass value="16.793507758"/>
-      <inertia ixx="0.217391101503" ixy="-0.00132873239126" ixz="-0.00228200226173" iyy="0.639432546734" iyz="-0.00138078263145" izz="0.62414077654"/>
+      <mass value="1e-6"/>
+      <inertia ixx="1e-6" ixy="1e-6" ixz="1e-6" iyy="1e-6" iyz="1e-6" izz="1e-6"/>
     </inertial>
   </link>
   <!-- Fixed joint to add dummy inertia link -->
-  <!-- <joint name="base_to_base_inertia" type="fixed">
+  <joint name="base_to_base_inertia" type="fixed">
     <parent link="base"/>
     <child link="base_inertia"/>
     <origin rpy="0 0 0" xyz="0 0 0"/>
-  </joint> -->
+  </joint>
   <!-- Dummy inertia link, because KDL cannot have inertia on the base link -->
-  <!-- <link name="base_inertia">
+  <link name="base_inertia">
     <inertial>
       <origin rpy="0 0 0" xyz="-0.001960558279 -0.001413217745 0.050207125344"/>
       <mass value="16.793507758"/>
       <inertia ixx="0.217391101503" ixy="-0.00132873239126" ixz="-0.00228200226173" iyy="0.639432546734" iyz="-0.00138078263145" izz="0.62414077654"/>
     </inertial>
-  </link> -->
+  </link>
   <!-- Xacro:Properties -->
   <!-- [kg * m^2] -->
   <!-- [A] -->
diff --git a/robots/icub_description/robots/icub.urdf b/robots/icub_description/robots/icub.urdf
index 3487940b41b3a690c2ed0d242d6e711c4fe7f1e3..e9179f93042c2776b54802d200c6db9ebb8013cc 100644
--- a/robots/icub_description/robots/icub.urdf
+++ b/robots/icub_description/robots/icub.urdf
@@ -1,5 +1,11 @@
+<?xml version="1.0" ?>
 <robot name="iCub">
-    <link name="base_link" />
+    <link name="base_link">
+        <inertial>
+            <mass value="1e-6"/>
+            <inertia ixx="1e-6" ixy="1e-6" ixz="1e-6" iyy="1e-6" iyz="1e-6" izz="1e-6"/>
+        </inertial>
+    </link>
     <link name="chest">
         <inertial>
             <mass value="4.81" />
diff --git a/robots/icub_description/robots/icub_reduced.urdf b/robots/icub_description/robots/icub_reduced.urdf
index 205d6c04a0f63a6b1c18de10993ea5002aeecb71..e9207bb669fcafbc7ed6342cb371a108296c91ef 100644
--- a/robots/icub_description/robots/icub_reduced.urdf
+++ b/robots/icub_description/robots/icub_reduced.urdf
@@ -1,5 +1,11 @@
+<?xml version="1.0" ?>
 <robot name="iCub">
-    <link name="base_link" />
+    <link name="base_link">
+        <inertial>
+            <mass value="1e-6"/>
+            <inertia ixx="1e-6" ixy="1e-6" ixz="1e-6" iyy="1e-6" iyz="1e-6" izz="1e-6"/>
+        </inertial>
+    </link>
     <link name="chest">
         <inertial>
             <mass value="4.81" />