diff --git a/CMakeLists.txt b/CMakeLists.txt
index ed2b4e253c8ac8d93e2adb62776e05ab492ea5c1..26bb27ab58b3e45232786d25e9b827b18b2a4117 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -118,6 +118,7 @@ SET(${PROJECT_NAME}_MATH_HEADERS
   math/fwd.hpp
   math/sincos.hpp
   math/quaternion.hpp
+  math/matrix.hpp
   )
 
 SET(${PROJECT_NAME}_TOOLS_HEADERS
diff --git a/src/math/matrix.hpp b/src/math/matrix.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..08e09bb417ac1806d3e708c5a54913d02b9f77de
--- /dev/null
+++ b/src/math/matrix.hpp
@@ -0,0 +1,34 @@
+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __math_matrix_hpp__
+#define __math_matrix_hpp__
+
+#include <Eigen/Dense>
+
+namespace se3
+{
+
+  template<typename Derived>
+  inline bool hasNaN(const Eigen::DenseBase<Derived> & m) 
+  {
+    return !((m.derived().array()==m.derived().array()).all());
+  }
+
+
+}
+#endif //#ifndef __math_matrix_hpp__
diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp
index c7cb4e2670a099fa7c9603f988606f2e75c548a1..d7799c7c3995ab80d659e4059f0ac62ec83fe19d 100644
--- a/src/parsers/urdf/model.cpp
+++ b/src/parsers/urdf/model.cpp
@@ -16,6 +16,7 @@
 // Pinocchio If not, see
 // <http://www.gnu.org/licenses/>.
 
+#include "pinocchio/math/matrix.hpp"
 #include "pinocchio/parsers/urdf.hpp"
 #include "pinocchio/parsers/urdf/utils.hpp"
 #include "pinocchio/multibody/model.hpp"
@@ -75,8 +76,8 @@ namespace se3
         // Reference to model.frames[fid] can has changed because the vector
         // may have been reallocated.
         if (model.frames[fid].parent > 0) {
-          assert (!model.inertias[model.frames[fid].parent].lever().hasNaN()
-              &&  !model.inertias[model.frames[fid].parent].inertia().data().hasNaN());
+          assert (!hasNaN(model.inertias[model.frames[fid].parent].lever())
+              &&  !hasNaN(model.inertias[model.frames[fid].parent].inertia().data()));
         }
       }