[C++][eigen] Remove call to eigen method hasNaN to ensure compatibility with...

[C++][eigen] Remove call to eigen method hasNaN to ensure compatibility with eigen 3.0.5. Provide our own implementation

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

src/math/matrix.hpp

+//
+// 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__

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()));
         }
       }