core: Allow to use SE3Tpl::isApprox with custom scalar

Author: jorisv

include/pinocchio/algorithm/constraints/cone-base.hpp

@@ -5,6 +5,8 @@
 #ifndef __pinocchio_algorithm_constraints_cone_base_hpp__
 #define __pinocchio_algorithm_constraints_cone_base_hpp__
 
+#include "pinocchio/math/matrix.hpp"
+
 #include "pinocchio/algorithm/constraints/fwd.hpp"
 #include "pinocchio/algorithm/constraints/set-base.hpp"
 
@@ -37,7 +39,7 @@ namespace pinocchio
     {
       assert(x.size() == scale.size() && " x and scale should have the same size.");
       assert(
-        scale.isApprox(scale(0) * VectorLikeIn2::Ones(scale.size()))
+        pinocchio::isApprox(scale, scale(0) * VectorLikeIn2::Ones(scale.size()))
         && "Only scalar scaling are supported.");
       PINOCCHIO_UNUSED_VARIABLE(scale); // the cone is preserved when scaled by a scalar
       return project(x, x_proj);

include/pinocchio/math/matrix.hpp

@@ -542,6 +542,63 @@ namespace pinocchio
     internal::arrayMinAlgo<M, Res>::run(x, min, res);
   }
 
+  namespace internal
+  {
+    template<
+      typename MatrixLike1,
+      typename MatrixLike2,
+      bool value = is_floating_point<typename MatrixLike1::Scalar>::value
+                   && is_floating_point<typename MatrixLike2::Scalar>::value>
+    struct isApproxAlgo
+    {
+      typedef typename MatrixLike1::RealScalar RealScalar;
+
+      static bool run(
+        const Eigen::MatrixBase<MatrixLike1> & m1,
+        const Eigen::MatrixBase<MatrixLike2> & m2,
+        const RealScalar & prec = Eigen::NumTraits<RealScalar>::dummy_precision())
+      {
+        return m1.isApprox(m2, prec);
+      }
+    };
+
+    template<typename MatrixLike1, typename MatrixLike2>
+    struct isApproxAlgo<MatrixLike1, MatrixLike2, false>
+    {
+      typedef typename MatrixLike1::RealScalar RealScalar;
+
+      static bool run(
+        const Eigen::MatrixBase<MatrixLike1> & m1,
+        const Eigen::MatrixBase<MatrixLike2> & m2,
+        const RealScalar & prec = Eigen::NumTraits<RealScalar>::dummy_precision())
+      {
+        PINOCCHIO_UNUSED_VARIABLE(m1);
+        PINOCCHIO_UNUSED_VARIABLE(m2);
+        PINOCCHIO_UNUSED_VARIABLE(prec);
+        return true;
+      }
+    };
+  } // namespace internal
+
+  ///
+  /// \brief Check whether two matrix are approximately the same
+  ///
+  /// \param[in] m1 Input matrix
+  /// \param[in] m2 Input matrix
+  /// \param[in] prec Required precision
+  ///
+  /// \returns true if m1 and m2 are approximately the same given precision prec.
+  ///
+  template<typename MatrixLike1, typename MatrixLike2>
+  inline bool isApprox(
+    const Eigen::MatrixBase<MatrixLike1> & m1,
+    const Eigen::MatrixBase<MatrixLike2> & m2,
+    const typename MatrixLike1::RealScalar & prec =
+      Eigen::NumTraits<typename MatrixLike1::Scalar>::dummy_precision())
+  {
+    return internal::isApproxAlgo<MatrixLike1, MatrixLike2>::run(m1, m2, prec);
+  }
+
   template<typename XprType, typename DestType>
   inline void evalTo(const XprType & xpr, DestType & dest)
   {

include/pinocchio/spatial/se3-tpl.hpp

@@ -10,6 +10,7 @@
 #include "pinocchio/spatial/se3-base.hpp"
 
 #include "pinocchio/math/quaternion.hpp"
+#include "pinocchio/math/matrix.hpp"
 #include "pinocchio/math/rotation.hpp"
 #include "pinocchio/spatial/cartesian-axis.hpp"
 
@@ -332,8 +333,8 @@ namespace pinocchio
       const SE3Tpl<Scalar, O2> & m2,
       const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
     {
-      return rotation().isApprox(m2.rotation(), prec)
-             && translation().isApprox(m2.translation(), prec);
+      return pinocchio::isApprox(rotation(), m2.rotation(), prec)
+             && pinocchio::isApprox(translation(), m2.translation(), prec);
     }
 
     bool isIdentity(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const