Merge pull request #1283 from aelkazdadi/is-floating-point

math: a more general floating point trait

src/math/fwd.hpp

@@ -11,6 +11,12 @@
 
 namespace pinocchio
 {
+
+  template <typename T>
+  struct is_floating_point : boost::is_floating_point<T>
+  {
+  };
+
   ///
   /// \brief Returns the value of PI according to the template parameters Scalar
   ///

src/math/matrix.hpp

@@ -22,7 +22,7 @@ namespace pinocchio
 
   namespace internal
   {
-    template<typename MatrixLike, bool value = boost::is_floating_point<typename MatrixLike::Scalar>::value>
+    template<typename MatrixLike, bool value = is_floating_point<typename MatrixLike::Scalar>::value>
     struct isZeroAlgo
     {
       typedef typename MatrixLike::Scalar Scalar;
@@ -98,7 +98,7 @@ namespace pinocchio
   
   namespace internal
   {
-    template<typename MatrixLike, bool value = boost::is_floating_point<typename MatrixLike::Scalar>::value>
+    template<typename MatrixLike, bool value = is_floating_point<typename MatrixLike::Scalar>::value>
     struct isUnitaryAlgo
     {
       typedef typename MatrixLike::Scalar Scalar;
@@ -146,7 +146,7 @@ namespace pinocchio
 
   namespace internal
   {
-    template<typename VectorLike, bool value = boost::is_floating_point<typename VectorLike::Scalar>::value>
+    template<typename VectorLike, bool value = is_floating_point<typename VectorLike::Scalar>::value>
     struct isNormalizedAlgo
     {
       typedef typename VectorLike::Scalar Scalar;
@@ -156,7 +156,7 @@ namespace pinocchio
                       const RealScalar & prec =
                       Eigen::NumTraits<RealScalar>::dummy_precision())
       {
-        return math::fabs(vec.norm() - RealScalar(1)) <= prec;
+        return math::fabs(static_cast<RealScalar>(vec.norm() - RealScalar(1))) <= prec;
       }
     };
     

src/math/multiprecision.hpp

@@ -14,6 +14,23 @@
 #include <boost/multiprecision/number.hpp>
 #include <Eigen/Dense>
 
+namespace pinocchio
+{
+// We check std::numeric_limits<_>::has_infinity to exclude integral, rational
+// and complex types
+template <typename Backend,
+          boost::multiprecision::expression_template_option ET>
+struct is_floating_point<boost::multiprecision::number<Backend, ET>>
+    : boost::integral_constant<
+          bool,
+          ((std::numeric_limits<
+                boost::multiprecision::number<Backend, ET>>::is_specialized and
+            std::numeric_limits<
+                boost::multiprecision::number<Backend, ET>>::has_infinity))>
+{
+};
+}  // namespace pinocchio
+
 namespace Eigen
 {
   namespace internal

src/math/quaternion.hpp

@@ -91,14 +91,14 @@ namespace pinocchio
       const Scalar N2 = q.squaredNorm();
 #ifndef NDEBUG
       const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
-      typedef apply_op_if<less_than_or_equal_to_op,boost::is_floating_point<Scalar>::value,true> static_leq;
-      assert(static_leq::op(math::fabs(N2-1.), epsilon));
+      typedef apply_op_if<less_than_or_equal_to_op,is_floating_point<Scalar>::value,true> static_leq;
+      assert(static_leq::op(math::fabs(static_cast<Scalar>(N2-Scalar(1))), epsilon));
 #endif
       const Scalar alpha = ((Scalar)3 - N2) / Scalar(2);
       PINOCCHIO_EIGEN_CONST_CAST(D,q).coeffs() *= alpha;
 #ifndef NDEBUG
       const Scalar M = Scalar(3) * math::pow(Scalar(1)-epsilon, ((Scalar)-Scalar(5))/Scalar(2)) / Scalar(4);
-      assert(static_leq::op(math::fabs(q.norm() - Scalar(1)),
+      assert(static_leq::op(math::fabs(static_cast<Scalar>(q.norm() - Scalar(1))),
                             math::max(M * sqrt(N2) * (N2 - Scalar(1))*(N2 - Scalar(1)) / Scalar(2), Eigen::NumTraits<Scalar>::dummy_precision())));
 #endif
     }
@@ -130,7 +130,7 @@ namespace pinocchio
     namespace internal
     {
 
-      template<typename Scalar, bool value = boost::is_floating_point<Scalar>::value>
+      template<typename Scalar, bool value = is_floating_point<Scalar>::value>
       struct quaternionbase_assign_impl;
       
       template<Eigen::DenseIndex i>

src/multibody/joint/joint-common-operations.hpp

@@ -17,7 +17,7 @@ namespace pinocchio
     ///
     /// \brief Operation called in JointModelBase<JointModel>::calc_aba
     ///
-    template<typename Scalar, bool is_floating_point = boost::is_floating_point<Scalar>::value>
+    template<typename Scalar, bool is_floating_point = pinocchio::is_floating_point<Scalar>::value>
     struct PerformStYSInversion
     {
       template<typename M1, typename M2>

src/multibody/joint/joint-free-flyer.hpp

@@ -227,8 +227,8 @@ namespace pinocchio
 
       ConstQuaternionMap quat(q_joint.template tail<4>().data());
       //assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
-      assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
-      
+      assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm()-1)) <= 1e-4);
+
       M.rotation(quat.matrix());
       M.translation(q_joint.template head<3>());
     }

src/multibody/joint/joint-spherical.hpp

@@ -417,13 +417,14 @@ namespace pinocchio
     template<typename ConfigVectorLike>
     inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
     {
+      typedef typename ConfigVectorLike::Scalar Scalar;
       EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
       typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
       typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
 
       ConstQuaternionMap quat(q_joint.derived().data());
       //assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
-      assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
+      assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm()-1)) <= 1e-4);
       
       M.rotation(quat.matrix());
       M.translation().setZero();

src/spatial/inertia.hpp

@@ -343,9 +343,9 @@ namespace pinocchio
                        const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
     {
       using math::fabs;
-      return fabs(mass() - other.mass()) <= prec
-          && lever().isApprox(other.lever(),prec)
-          && inertia().isApprox(other.inertia(),prec);
+      return fabs(static_cast<Scalar>(mass() - other.mass())) <= prec
+      && lever().isApprox(other.lever(),prec)
+      && inertia().isApprox(other.inertia(),prec);
     }
     
     bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const