Verified Commit 138c3e49 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

core: force casting of expression in fabs argument

parent 5bf00cc5
Pipeline #10923 passed with stage
in 212 minutes and 48 seconds
......@@ -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;
}
};
......
......@@ -92,13 +92,13 @@ namespace pinocchio
#ifndef NDEBUG
const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::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(N2-1.), epsilon));
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
}
......
......@@ -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