Verified Commit de604089 authored by Justin Carpentier's avatar Justin Carpentier Committed by Justin Carpentier
Browse files

[All] Define math::fabs function

This is mandatatory to use AutoDiff
parent 04dd7fb5
......@@ -19,6 +19,7 @@
#define __se3_math_fwd_hpp__
#include "pinocchio/fwd.hpp"
#include <math.h>
#include <boost/math/constants/constants.hpp>
namespace se3
......@@ -34,7 +35,14 @@ namespace se3
/// The value of PI for double scalar type
const double PId = PI<double>();
namespace math
{
using std::fabs;
#ifdef PINOCCHIO_WITH_CPPAD_SUPPORT
using CppAD::fabs;
#endif
}
}
#endif //#ifndef __se3_math_fwd_hpp__
......@@ -91,13 +91,13 @@ namespace se3
const Scalar N2 = q.squaredNorm();
#ifndef NDEBUG
const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
assert(std::fabs(N2-1.) <= epsilon);
assert(math::fabs(N2-1.) <= epsilon);
#endif
const Scalar alpha = ((Scalar)3 - N2) / Scalar(2);
const_cast <Eigen::QuaternionBase<D> &> (q).coeffs() *= alpha;
#ifndef NDEBUG
const Scalar M = Scalar(3) * std::pow(Scalar(1)-epsilon, ((Scalar)-Scalar(5))/Scalar(2)) / Scalar(4);
assert(std::fabs(q.norm() - Scalar(1)) <=
assert(math::fabs(q.norm() - Scalar(1)) <=
std::max(M * sqrt(N2) * (N2 - Scalar(1))*(N2 - Scalar(1)) / Scalar(2), Eigen::NumTraits<Scalar>::dummy_precision()));
#endif
}
......
......@@ -240,8 +240,8 @@ namespace se3
typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
ConstQuaternionMap quat(q_joint.template tail<4>().data());
//assert(std::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
assert(std::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
//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);
M.rotation(quat.matrix());
M.translation(q_joint.template head<3>());
......
......@@ -428,8 +428,8 @@ namespace se3
typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
ConstQuaternionMap quat(q_joint.derived().data());
//assert(std::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
assert(std::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
//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);
M.rotation(quat.matrix());
M.translation().setZero();
......
......@@ -79,7 +79,7 @@ namespace se3
Scalar cv,sv; SINCOS(omega, &sv, &cv);
const_cast<Matrix2Like &>(R.derived()) << cv, -sv, sv, cv;
if (std::fabs(omega) > 1e-14)
if (math::fabs(omega) > 1e-14)
{
typename EIGEN_PLAIN_TYPE(Vector2Like) vcross(-v(1), v(0));
vcross /= omega;
......@@ -124,7 +124,7 @@ namespace se3
typedef typename Matrix2Like::Scalar Scalar1;
Scalar1 t = SO2_t::log(R);
const Scalar1 tabs = std::fabs(t);
const Scalar1 tabs = math::fabs(t);
const Scalar1 t2 = t*t;
Scalar1 alpha;
if (tabs < 1e-4)
......@@ -157,7 +157,7 @@ namespace se3
typedef typename Matrix2Like::Scalar Scalar1;
Scalar1 t = SO2_t::log(R);
const Scalar1 tabs = std::fabs(t);
const Scalar1 tabs = math::fabs(t);
Scalar1 alpha, alpha_dot;
if (tabs < 1e-4)
{
......
......@@ -334,7 +334,7 @@ namespace se3
Vector3 w(log3(R,t));
const Scalar t2 = t*t;
Scalar alpha, beta;
if (std::fabs(t) < 1e-4)
if (math::fabs(t) < 1e-4)
{
alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720);
beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
......
......@@ -22,6 +22,7 @@
#include <Eigen/Core>
#include <iostream>
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/spatial/symmetric3.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
......@@ -294,7 +295,7 @@ namespace se3
bool isApprox_impl(const InertiaTpl & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
using std::fabs;
using math::fabs;
return fabs(mass() - other.mass()) <= prec
&& lever().isApprox(other.lever(),prec)
&& inertia().isApprox(other.inertia(),prec);
......
......@@ -69,7 +69,7 @@ template<typename Matrix>
void filterValue(MatrixBase<Matrix> & mat, typename Matrix::Scalar value)
{
for(int k = 0; k < mat.size(); ++k)
mat.derived().data()[k] = std::fabs(mat.derived().data()[k]) <= value?0:mat.derived().data()[k];
mat.derived().data()[k] = math::fabs(mat.derived().data()[k]) <= value?0:mat.derived().data()[k];
}
struct FiniteDiffJoint
......
......@@ -89,7 +89,7 @@ void test_joint_methods(const JointModelBase<JointModel> & jmodel, JointModelCom
//
// const double alpha = 0.2;
// BOOST_CHECK(jmodel_composite.interpolate(q1,q2,alpha).isApprox(jmodel.interpolate(q1,q2,alpha)));
// BOOST_CHECK(std::fabs(jmodel_composite.distance(q1,q2)-jmodel.distance(q1,q2))<= NumTraits<double>::dummy_precision());
// BOOST_CHECK(math::fabs(jmodel_composite.distance(q1,q2)-jmodel.distance(q1,q2))<= NumTraits<double>::dummy_precision());
// }
Inertia::Matrix6 I1(Inertia::Random().matrix());
......
......@@ -113,7 +113,7 @@ void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &)
// Check distance
Scalar dist = LieGroupType().distance(q1,q2);
BOOST_CHECK_MESSAGE(dist > 0., "distance - wrong results");
BOOST_CHECK_SMALL(std::fabs(dist-q1_dot.norm()), 10*prec);
BOOST_CHECK_SMALL(math::fabs(dist-q1_dot.norm()), 10*prec);
std::string error_prefix("LieGroup");
error_prefix += " on joint " + jmodel.shortname();
......
Supports Markdown
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