diff --git a/kdl_parser/src/kdl_parser.cpp b/kdl_parser/src/kdl_parser.cpp
index 4b8c3ab..e1efd51 100644
--- a/kdl_parser/src/kdl_parser.cpp
+++ b/kdl_parser/src/kdl_parser.cpp
@@ -96,11 +96,34 @@ Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
 RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
 {
   Frame origin = toKdl(i->origin);
-  // kdl specifies the inertia in the reference frame of the link, the urdf specifies the inertia in the inertia reference frame
-  return origin.M * RigidBodyInertia(i->mass, origin.p, RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz));
+  
+  // the mass is frame indipendent 
+  double kdl_mass = i->mass;
+  
+  // kdl and urdf both specify the com position in the reference frame of the link
+  Vector kdl_com = origin.p;
+  
+  // kdl specifies the inertia matrix in the reference frame of the link, 
+  // while the urdf specifies the inertia matrix in the inertia reference frame
+  RotationalInertia urdf_inertia = 
+    RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz);
+    
+  // Rotation operators are not defined for rotational inertia,
+  // so we use the RigidBodyInertia operators (with com = 0) as a workaround
+  RigidBodyInertia kdl_inertia_wrt_com_workaround =
+    origin.M *RigidBodyInertia(0, Vector::Zero(), urdf_inertia);
+  
+  // Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com
+  // while the getRotationalInertia method returns the 3d inertia wrt the frame origin
+  // (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match)
+  RotationalInertia kdl_inertia_wrt_com = 
+    kdl_inertia_wrt_com_workaround.getRotationalInertia();
+    
+  return RigidBodyInertia(kdl_mass,kdl_com,kdl_inertia_wrt_com);  
 }
 
 
+
 // recursive function to walk through tree
 bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
 {