diff --git a/cmake b/cmake
index 2ef482debaa2b39da11902acda6bb9fcee461fca..46dc4a57521bde14ea75c959b6b4f887af50c65d 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 2ef482debaa2b39da11902acda6bb9fcee461fca
+Subproject commit 46dc4a57521bde14ea75c959b6b4f887af50c65d
diff --git a/include/eigenpy/angle-axis.hpp b/include/eigenpy/angle-axis.hpp
index e7aca37a4e4a366ff3151ce4a57091bd263e0e4d..2946866262d753613752ff206af6cfc9dbada56c 100644
--- a/include/eigenpy/angle-axis.hpp
+++ b/include/eigenpy/angle-axis.hpp
@@ -19,18 +19,23 @@ namespace eigenpy
template<typename AngleAxis> class AngleAxisVisitor;
- namespace internal
+ template<typename Scalar>
+ struct call< Eigen::AngleAxis<Scalar> >
{
- template<typename Scalar>
- struct call_expose< Eigen::AngleAxis<Scalar> >
+ typedef Eigen::AngleAxis<Scalar> AngleAxis;
+ static inline void expose()
{
- typedef Eigen::AngleAxis<Scalar> type;
- static inline void run()
- {
- AngleAxisVisitor<type>::expose();
- }
- };
- } // namespace internal
+ AngleAxisVisitor<AngleAxis>::expose();
+ }
+
+ static inline bool isApprox(const AngleAxis & self, const AngleAxis & other,
+ const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
+ {
+ return self.isApprox(other,prec);
+ }
+ };
+
+ BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxAngleAxis_overload,call<Eigen::AngleAxisd>::isApprox,2,3)
template<typename AngleAxis>
class AngleAxisVisitor
@@ -74,11 +79,11 @@ namespace eigenpy
bp::arg("Sets *this from a 3x3 rotation matrix."),bp::return_self<>())
.def("toRotationMatrix",&AngleAxis::toRotationMatrix,"Constructs and returns an equivalent 3x3 rotation matrix.")
.def("matrix",&AngleAxis::matrix,"Returns an equivalent rotation matrix.")
- .def("isApprox",(bool (AngleAxis::*)(const AngleAxis &))&AngleAxis::isApprox,
- "Returns true if *this is approximately equal to other.")
- .def("isApprox",(bool (AngleAxis::*)(const AngleAxis &, const Scalar prec))&AngleAxis::isApprox,
- bp::args("other","prec"),
- "Returns true if *this is approximately equal to other, within the precision determined by prec.")
+
+ .def("isApprox",
+ &call<AngleAxis>::isApprox,
+ isApproxAngleAxis_overload(bp::args("other","prec"),
+ "Returns true if *this is approximately equal to other, within the precision determined by prec."))
/* --- Operators --- */
.def(bp::self * bp::other<Vector3>())
@@ -101,7 +106,8 @@ namespace eigenpy
{ self.angle() = angle; }
static bool __eq__(const AngleAxis & u, const AngleAxis & v)
- { return u.isApprox(v); }
+ { return u.axis() == v.axis() && v.angle() == u.angle(); }
+
static bool __ne__(const AngleAxis & u, const AngleAxis & v)
{ return !__eq__(u,v); }
diff --git a/include/eigenpy/expose.hpp b/include/eigenpy/expose.hpp
index 24158cd2d5a9831964e97cde7ed5ea1639a47d06..9fa8e51bee3963e3418ce543fa613bf12b8b1a21 100644
--- a/include/eigenpy/expose.hpp
+++ b/include/eigenpy/expose.hpp
@@ -1,5 +1,5 @@
/*
- * Copyright 2019, INRIA
+ * Copyright 2019 INRIA
*/
#ifndef __eigenpy_expose_hpp__
@@ -9,18 +9,15 @@
namespace eigenpy
{
- namespace internal
+ ///
+ /// \brief Allows a template specialization.
+ ///
+ template<typename T>
+ struct call
{
- ///
- /// \brief Allows a template specialization.
- ///
- template<typename T>
- struct call_expose
- {
- static inline void run() { T::expose(); }
- };
- } // namespace internal
-
+ static inline void expose() { T::expose(); }
+ };
+
///
/// \brief Call the expose function of a given type T.
///
@@ -28,7 +25,7 @@ namespace eigenpy
inline void expose()
{
if(!register_symbolic_link_to_registered_type<T>())
- internal::call_expose<T>::run();
+ call<T>::expose();
}
}
diff --git a/include/eigenpy/quaternion.hpp b/include/eigenpy/quaternion.hpp
index 610a0347061ba8f8ea4a6b7a1249e441afc80704..4b5120b4ef6ca169e253a6045de7b6ee81bec2d0 100644
--- a/include/eigenpy/quaternion.hpp
+++ b/include/eigenpy/quaternion.hpp
@@ -31,19 +31,24 @@ namespace eigenpy
template<typename QuaternionDerived> class QuaternionVisitor;
- namespace internal
+ template<typename Scalar, int Options>
+ struct call< Eigen::Quaternion<Scalar,Options> >
{
- template<typename Scalar, int Options>
- struct call_expose< Eigen::Quaternion<Scalar,Options> >
+ typedef Eigen::Quaternion<Scalar,Options> Quaternion;
+ static inline void expose()
{
- typedef Eigen::Quaternion<Scalar,Options> type;
- static inline void run()
- {
- QuaternionVisitor<type>::expose();
- }
- };
- } // namespace internal
-
+ QuaternionVisitor<Quaternion>::expose();
+ }
+
+ static inline bool isApprox(const Quaternion & self, const Quaternion & other,
+ const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
+ {
+ return self.isApprox(other,prec);
+ }
+ };
+
+ BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxQuaternion_overload,call<Eigen::Quaterniond>::isApprox,2,3)
+
template<typename Quaternion>
class QuaternionVisitor
: public bp::def_visitor< QuaternionVisitor<Quaternion> >
@@ -65,13 +70,22 @@ namespace eigenpy
{
cl
.def(bp::init<>("Default constructor"))
- .def(bp::init<Vector4>((bp::arg("Vec4: a 4D vector representing quaternion coefficients")),"Initialize from a vector 4D."))
- .def(bp::init<Matrix3>((bp::arg("R: a rotation matrix")),"Initialize from rotation matrix."))
- .def(bp::init<AngleAxis>((bp::arg("aa: angle axis")),"Initialize from an angle axis."))
- .def(bp::init<Quaternion>((bp::arg("quaternion")),"Copy constructor."))
+ .def(bp::init<Vector4>((bp::arg("vec4")),
+ "Initialize from a vector 4D.\n"
+ "\tvec4 : a 4D vector representing quaternion coefficients in the order xyzw."))
+ .def(bp::init<Matrix3>((bp::arg("R")),
+ "Initialize from rotation matrix.\n"
+ "\tR : a rotation matrix 3x3."))
+ .def(bp::init<AngleAxis>((bp::arg("aa")),
+ "Initialize from an angle axis.\n"
+ "\taa: angle axis object."))
+ .def(bp::init<Quaternion>((bp::arg("quat")),
+ "Copy constructor.\n"
+ "\tquat: a quaternion."))
.def("__init__",bp::make_constructor(&QuaternionVisitor::FromTwoVectors,
bp::default_call_policies(),
- (bp::arg("u: a 3D vector"),bp::arg("v: a 3D vector"))),"Initialize from two vectors u andv")
+ (bp::arg("u: a 3D vector"),bp::arg("v: a 3D vector"))),
+ "Initialize from two vectors u and v")
.def(bp::init<Scalar,Scalar,Scalar,Scalar>
((bp::arg("w"),bp::arg("x"),bp::arg("y"),bp::arg("z")),
"Initialize from coefficients.\n\n"
@@ -91,14 +105,10 @@ namespace eigenpy
&QuaternionVisitor::getCoeff<3>,
&QuaternionVisitor::setCoeff<3>,"The w coefficient.")
-// .def("isApprox",(bool (Quaternion::*)(const Quaternion &))&Quaternion::template isApprox<Quaternion>,
-// "Returns true if *this is approximately equal to other.")
-// .def("isApprox",(bool (Quaternion::*)(const Quaternion &, const Scalar prec))&Quaternion::template isApprox<Quaternion>,
-// "Returns true if *this is approximately equal to other, within the precision determined by prec..")
- .def("isApprox",(bool (*)(const Quaternion &))&isApprox,
- "Returns true if *this is approximately equal to other.")
- .def("isApprox",(bool (*)(const Quaternion &, const Scalar prec))&isApprox,
- "Returns true if *this is approximately equal to other, within the precision determined by prec..")
+ .def("isApprox",
+ &call<Quaternion>::isApprox,
+ isApproxQuaternion_overload(bp::args("other","prec"),
+ "Returns true if *this is approximately equal to other, within the precision determined by prec."))
/* --- Methods --- */
.def("coeffs",(const Vector4 & (Quaternion::*)()const)&Quaternion::coeffs,
@@ -106,7 +116,7 @@ namespace eigenpy
.def("matrix",&Quaternion::matrix,"Returns an equivalent 3x3 rotation matrix. Similar to toRotationMatrix.")
.def("toRotationMatrix",&Quaternion::toRotationMatrix,"Returns an equivalent 3x3 rotation matrix.")
- .def("setFromTwoVectors",&setFromTwoVectors,((bp::arg("a"),bp::arg("b"))),"Set *this to be the quaternion which transform a into b through a rotation."
+ .def("setFromTwoVectors",&setFromTwoVectors,((bp::arg("a"),bp::arg("b"))),"Set *this to be the quaternion which transforms a into b through a rotation."
,bp::return_self<>())
.def("conjugate",&Quaternion::conjugate,"Returns the conjugated quaternion. The conjugate of a quaternion represents the opposite rotation.")
.def("inverse",&Quaternion::inverse,"Returns the quaternion describing the inverse rotation.")
@@ -145,13 +155,11 @@ namespace eigenpy
// "Returns the quaternion which transform a into b through a rotation.")
.def("FromTwoVectors",&FromTwoVectors,
bp::args("a","b"),
- "Returns the quaternion which transform a into b through a rotation.",
+ "Returns the quaternion which transforms a into b through a rotation.",
bp::return_value_policy<bp::manage_new_object>())
.staticmethod("FromTwoVectors")
.def("Identity",&Quaternion::Identity,"Returns a quaternion representing an identity rotation.")
.staticmethod("Identity")
-
-
;
}
private:
@@ -174,16 +182,10 @@ namespace eigenpy
Quaternion* q(new Quaternion); q->setFromTwoVectors(u,v);
return q;
}
-
- static bool isApprox(const Quaternion & self, const Quaternion & other,
- const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision)
- {
- return self.isApprox(other,prec);
- }
- static bool __eq__(const Quaternion& u, const Quaternion& v)
+ static bool __eq__(const Quaternion & u, const Quaternion & v)
{
- return u.isApprox(v,1e-9);
+ return u.coeffs() == v.coeffs();
}
static bool __ne__(const Quaternion& u, const Quaternion& v)
diff --git a/unittest/python/test_geometry.py b/unittest/python/test_geometry.py
index 07d91a33fdd699b786989ba3953f2d9d3fc1fc8f..d14ed2060fe68d47aa732b6863abcd220eeaefaa 100644
--- a/unittest/python/test_geometry.py
+++ b/unittest/python/test_geometry.py
@@ -1,5 +1,6 @@
from __future__ import print_function
+from eigenpy import *
from geometry import *
import numpy as np
from numpy import cos,sin
@@ -20,8 +21,10 @@ assert(isapprox(np.linalg.norm(q.coeffs()),1))
r = AngleAxis(q)
q2 = Quaternion(r)
-assert(q==q2)
+assert(q==q)
assert(isapprox(q.coeffs(),q2.coeffs()))
+assert(q2.isApprox(q2))
+assert(q2.isApprox(q2,1e-2))
Rq = q.matrix()
Rr = r.matrix()
@@ -29,7 +32,7 @@ assert(isapprox(Rq*Rq.T,np.eye(3)))
assert(isapprox(Rr,Rq))
qR = Quaternion(Rr)
-assert(q==qR)
+assert(q.isApprox(qR))
assert(isapprox(q.coeffs(),qR.coeffs()))
assert(isapprox(qR[3],1./np.sqrt(30)))
@@ -45,6 +48,8 @@ if verbose: print("Rx(.1) = \n\n",r.matrix(),"\n")
assert( isapprox(r.matrix()[2,2],cos(r.angle)))
assert( isapprox(r.axis,np.matrix("1;0;0")) )
assert( isapprox(r.angle,0.1) )
+assert(r.isApprox(r))
+assert(r.isApprox(r,1e-2))
r.axis = np.matrix([0,1,0],np.double).T
assert( isapprox(r.matrix()[0,0],cos(r.angle)))