Unverified Commit 9a0c8d0f authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

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

math: a more general floating point trait
parents 8abc2c93 138c3e49
Pipeline #10926 passed with stage
in 144 minutes and 2 seconds
......@@ -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
///
......
......@@ -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;
}
};
......
......@@ -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
......
......@@ -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>
......
......@@ -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>
......
......@@ -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>());
}
......
......@@ -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();
......
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment