diff --git a/src/multibody/joint/joint-revolute-unbounded.hpp b/src/multibody/joint/joint-revolute-unbounded.hpp index 70465dc4ccfdbe09fc1d3e3a7808d2e4eee963a9..4ce217e4e9c2506893ee86bfe692d90f0b003df9 100644 --- a/src/multibody/joint/joint-revolute-unbounded.hpp +++ b/src/multibody/joint/joint-revolute-unbounded.hpp @@ -236,7 +236,7 @@ namespace se3 double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const { - return fabs(difference_impl(q0,q1)[0]); + return difference_impl(q0,q1).norm(); } ConfigVector_t neutralConfiguration_impl() const