diff --git a/src/algorithm/joint-configuration.hpp b/src/algorithm/joint-configuration.hpp
index ef29e29f9743251a137387a4441aec2af5988d08..a9de210cdf684197a68eacbdbb354b9c4f3ad348 100644
--- a/src/algorithm/joint-configuration.hpp
+++ b/src/algorithm/joint-configuration.hpp
@@ -112,6 +112,20 @@ namespace se3
*/
inline void normalize(const Model & model,
Eigen::VectorXd & q);
+
+ /**
+ * @brief Return true if the given configurations are equivalents
+ * \warning Two configurations can be equivalent but not equally coefficient wise (e.g for quaternions)
+ *
+ * @param[in] model Model
+ * @param[in] q1 The first configuraiton to compare
+ * @param[in] q2 The Second configuraiton to compare
+ *
+ * @return Wheter the configurations are equivalent or not
+ */
+ inline bool isSameConfiguration(const Model & model,
+ const Eigen::VectorXd & q1,
+ const Eigen::VectorXd & q2);
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
@@ -332,6 +346,40 @@ namespace se3
}
+ struct IsSameConfigurationStep : public fusion::JointModelVisitor<IsSameConfigurationStep>
+ {
+ typedef boost::fusion::vector<bool &,
+ const Eigen::VectorXd &,
+ const Eigen::VectorXd &> ArgsType;
+
+ JOINT_MODEL_VISITOR_INIT(IsSameConfigurationStep);
+
+ template<typename JointModel>
+ static void algo(const se3::JointModelBase<JointModel> & jmodel,
+ bool & isSame,
+ const Eigen::VectorXd & q1,
+ const Eigen::VectorXd & q2)
+ {
+ isSame = jmodel.isSameConfiguration(q1,q2);
+ }
+ };
+
+ inline bool
+ isSameConfiguration(const Model & model,
+ const Eigen::VectorXd & q1,
+ const Eigen::VectorXd & q2)
+ {
+ bool result = false;
+ for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
+ {
+ IsSameConfigurationStep::run(model.joints[i], IsSameConfigurationStep::ArgsType (result,q1,q2));
+ if( !result )
+ return false;
+ }
+ return true;
+ }
+
+
} // namespace se3
#endif // ifndef __se3_joint_configuration_hpp__
diff --git a/src/multibody/joint/joint-base.hpp b/src/multibody/joint/joint-base.hpp
index 68c3c4505d4a09e47d35355b7e6c1b6b40b483e5..d5f2a83a410114d933c13dffb516465cc6d28005 100644
--- a/src/multibody/joint/joint-base.hpp
+++ b/src/multibody/joint/joint-base.hpp
@@ -68,6 +68,7 @@ namespace se3
#define SE3_JOINT_TYPEDEF_ARG(prefix) \
typedef int Index; \
+ typedef prefix traits<JointDerived>::Scalar Scalar; \
typedef prefix traits<JointDerived>::JointDataDerived JointDataDerived; \
typedef prefix traits<JointDerived>::JointModelDerived JointModelDerived; \
typedef prefix traits<JointDerived>::Constraint_t Constraint_t; \
@@ -92,6 +93,7 @@ namespace se3
#define SE3_JOINT_TYPEDEF_NOARG() \
typedef int Index; \
+ typedef traits<JointDerived>::Scalar Scalar; \
typedef traits<JointDerived>::JointDataDerived JointDataDerived; \
typedef traits<JointDerived>::JointModelDerived JointModelDerived; \
typedef traits<JointDerived>::Constraint_t Constraint_t; \
@@ -111,6 +113,7 @@ namespace se3
#define SE3_JOINT_TYPEDEF_ARG(prefix) \
typedef int Index; \
+ typedef prefix traits<JointDerived>::Scalar Scalar;
typedef prefix traits<JointDerived>::JointDataDerived JointDataDerived; \
typedef prefix traits<JointDerived>::JointModelDerived JointModelDerived; \
typedef prefix traits<JointDerived>::Constraint_t Constraint_t; \
@@ -135,6 +138,7 @@ namespace se3
#define SE3_JOINT_TYPEDEF_ARG() \
typedef int Index; \
+ typedef typename traits<JointDerived>::Scalar Scalar; \
typedef typename traits<JointDerived>::JointDataDerived JointDataDerived; \
typedef typename traits<JointDerived>::JointModelDerived JointModelDerived; \
typedef typename traits<JointDerived>::Constraint_t Constraint_t; \
@@ -193,6 +197,22 @@ namespace se3
JointDataDense<NQ, NV> toDense() const { return static_cast<const JointDataDerived*>(this)->toDense_impl(); }
+ protected:
+ /// Default constructor: protected.
+ ///
+ /// Prevent the construction of stand-alone JointDataBase.
+ inline JointDataBase() {} // TODO: default value should be set to -1
+ /// Copy constructor: protected.
+ ///
+ /// Copy of stand-alone JointDataBase are prevented, but can be used from inhereting
+ /// objects. Copy is done by calling copy operator.
+ inline JointDataBase( const JointDataBase& clone) { *this = clone; }
+ /// Copy operator: protected.
+ ///
+ /// Copy of stand-alone JointDataBase are prevented, but can be used from inhereting
+ /// objects.
+ inline JointDataBase& operator= (const JointDataBase&) { return *this; }
+
}; // struct JointDataBase
template<int NV>
@@ -356,6 +376,15 @@ namespace se3
*/
void normalize_impl(Eigen::VectorXd &) const { }
+ /**
+ * @brief Check if two configurations are equivalent within the given precision
+ *
+ * @param[in] q1 Configuration 1 (size full model.nq)
+ * @param[in] q2 Configuration 2 (size full model.nq)
+ */
+ bool isSameConfiguration(const Eigen::VectorXd & q1, const Eigen::VectorXd & q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ { return derived().isSameConfiguration_impl(q1,q2, prec); }
+
JointIndex i_id; // ID of the joint in the multibody list.
int i_q; // Index of the joint configuration in the joint configuration vector.
int i_v; // Index of the joint velocity in the joint velocity vector.
@@ -452,6 +481,29 @@ namespace se3
JointModelDense<NQ, NV> toDense() const { return derived().toDense_impl(); }
+ protected:
+
+ /// Default constructor: protected.
+ ///
+ /// Prevent the construction of stand-alone JointModelBase.
+ inline JointModelBase() {} // TODO: default value should be set to -1
+ /// Copy constructor: protected.
+ ///
+ /// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting
+ /// objects. Copy is done by calling copy operator.
+ inline JointModelBase( const JointModelBase& clone) { *this = clone; }
+ /// Copy operator: protected.
+ ///
+ /// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting
+ /// objects.
+ inline JointModelBase& operator= (const JointModelBase& clone)
+ {
+ i_id = clone.i_id;
+ i_q = clone.i_q;
+ i_v = clone.i_v;
+ return *this;
+ }
+
}; // struct JointModelBase
} // namespace se3
diff --git a/src/multibody/joint/joint-basic-visitors.hpp b/src/multibody/joint/joint-basic-visitors.hpp
index c1e15ec9df08c0ebe223b015ebfafe30ebc40fec..9aa612990297fa4b1dc0716b4d73823922386e28 100644
--- a/src/multibody/joint/joint-basic-visitors.hpp
+++ b/src/multibody/joint/joint-basic-visitors.hpp
@@ -173,6 +173,20 @@ namespace se3
inline void normalize(const JointModelVariant & jmodel,
Eigen::VectorXd & q);
+ /**
+ * @brief Visit a JointModelVariant through JointIsSameConfigurationVisitor to determine
+ * if the two given configurations are equivalent or not.
+ *
+ * @param[in] jmodel The JointModelVariant
+ * @param[in] q1 Configuration 1
+ * @param[in] q2 Configuration 2
+ *
+ * @return Wheter the two configurations are equivalent
+ */
+ inline bool isSameConfiguration(const JointModelVariant & jmodel,
+ const Eigen::VectorXd & q1,
+ const Eigen::VectorXd & q2);
+
/**
* @brief Visit a JointModelVariant through JointNvVisitor to get the dimension of
* the joint tangent space
diff --git a/src/multibody/joint/joint-basic-visitors.hxx b/src/multibody/joint/joint-basic-visitors.hxx
index 616442203a691273889454611f89f36d7b741e4f..ecb549b4ea5f511dc97d3660356501e682d1acca 100644
--- a/src/multibody/joint/joint-basic-visitors.hxx
+++ b/src/multibody/joint/joint-basic-visitors.hxx
@@ -265,6 +265,31 @@ namespace se3
JointNormalizeVisitor::run(jmodel, q);
}
+ /**
+ * @brief JointIsSameConfigurationVisitor visitor
+ */
+ class JointIsSameConfigurationVisitor: public boost::static_visitor<bool>
+ {
+ public:
+ const Eigen::VectorXd & q1;
+ const Eigen::VectorXd & q2;
+
+ JointIsSameConfigurationVisitor(const Eigen::VectorXd & q1,const Eigen::VectorXd & q2) : q1(q1), q2(q2) {}
+
+ template<typename D>
+ bool operator()(const JointModelBase<D> & jmodel) const
+ { return jmodel.isSameConfiguration(q1, q2); }
+
+ static bool run(const JointModelVariant & jmodel, const Eigen::VectorXd & q1, const Eigen::VectorXd & q2)
+ { return boost::apply_visitor( JointIsSameConfigurationVisitor(q1,q2), jmodel ); }
+ };
+
+ inline bool isSameConfiguration(const JointModelVariant & jmodel,
+ const Eigen::VectorXd & q1,
+ const Eigen::VectorXd & q2)
+ {
+ return JointIsSameConfigurationVisitor::run(jmodel, q1, q2);
+ }
/**
* @brief JointDifferenceVisitor visitor
diff --git a/src/multibody/joint/joint-dense.hpp b/src/multibody/joint/joint-dense.hpp
index c8647ff88f0ada66f62989b1d7e9b42b1b80045a..55e1f5434c28f82b7475be5e0ea6c07aac099b0d 100644
--- a/src/multibody/joint/joint-dense.hpp
+++ b/src/multibody/joint/joint-dense.hpp
@@ -30,6 +30,7 @@ namespace se3
NQ = _NQ, // pb
NV = _NV
};
+ typedef double Scalar;
typedef JointDataDense<_NQ, _NV> JointDataDerived;
typedef JointModelDense<_NQ, _NV> JointModelDerived;
typedef ConstraintXd Constraint_t;
@@ -202,6 +203,11 @@ namespace se3
assert(false && "JointModelDense is read-only, should not perform any calc");
}
+ bool isSameConfiguration_impl(const Eigen::VectorXd &, const Eigen::VectorXd &, const Scalar & = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ assert(false && "JointModelDense is read-only, should not perform any calc");
+ }
+
JointModelDense() {}
JointModelDense(JointIndex idx, int idx_q, int idx_v)
{
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index 488a80b9a8f622bff195e9d3e3d182e41ef37f45..f5f9e444c2442cf95e27ebb7c0de4037810829a6 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -143,6 +143,7 @@ namespace se3
NQ = 7,
NV = 6
};
+ typedef double Scalar;
typedef JointDataFreeFlyer JointDataDerived;
typedef JointModelFreeFlyer JointModelDerived;
typedef ConstraintIdentity Constraint_t;
@@ -201,7 +202,6 @@ namespace se3
using JointModelBase<JointModelFreeFlyer>::idx_v;
using JointModelBase<JointModelFreeFlyer>::setIndexes;
typedef Motion::Vector3 Vector3;
- typedef double Scalar;
JointDataDerived createData() const { return JointDataDerived(); }
@@ -255,10 +255,9 @@ namespace se3
I.setZero();
}
- ConfigVector_t::Scalar finiteDifferenceIncrement() const
+ Scalar finiteDifferenceIncrement() const
{
using std::sqrt;
- typedef ConfigVector_t::Scalar Scalar;
return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
}
@@ -381,6 +380,19 @@ namespace se3
q.segment<4>(idx_q()+3).normalize();
}
+ bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
+
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.segment<NQ> (idx_q ());
+
+ ConstQuaternionMap_t quat1(q_1.tail<4> ().data());
+ ConstQuaternionMap_t quat2(q_2.tail<4> ().data());
+
+ return ( q_1.head<3>().isApprox(q_2.head<3>(), prec) && defineSameRotation(quat1,quat2) );
+ }
+
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp
index 6faa4b70a131e7f3a082b555695c51cf6a52d408..5064c6ecb135f71e058f296d4a73c93c0f2fc36e 100644
--- a/src/multibody/joint/joint-planar.hpp
+++ b/src/multibody/joint/joint-planar.hpp
@@ -273,6 +273,7 @@ namespace se3
NQ = 3,
NV = 3
};
+ typedef double Scalar;
typedef JointDataPlanar JointDataDerived;
typedef JointModelPlanar JointModelDerived;
typedef ConstraintPlanar Constraint_t;
@@ -394,31 +395,52 @@ namespace se3
{
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.segment<NV> (idx_v ());
- typedef Transformation_t::Matrix3 Matrix3;
-
+ typedef Eigen::Matrix<double, 2, 2> Matrix22;
+ typedef Eigen::Matrix<double, 2, 1> Vector2;
+
double c0,s0; SINCOS (q(2), &s0, &c0);
- Matrix3 R0(Matrix3::Identity());
- R0.topLeftCorner <2,2> () << c0, -s0, s0, c0;
+ Matrix22 R0;
+ R0 << c0, -s0, s0, c0;
+ const double& t = q_dot[2];
+ const double theta = std::fabs(t);
+
ConfigVector_t res(q);
- if(std::fabs(q_dot(2)) > 1e-14)
+ if(theta > 1e-14)
{
- ConfigVector_t tmp(ConfigVector_t::Zero());
-
- double c1,s1; SINCOS (q_dot(2), &s1, &c1);
- const double c_coeff = (1.-c1)/q_dot(2);
- tmp.head<2>() = s1/q_dot(2)*q_dot.head<2>();
- tmp(0) -= c_coeff*q_dot(1);
- tmp(1) += c_coeff*q_dot(0);
-
- res.head<2>() += R0.topLeftCorner<2,2>()*tmp.head<2>();
- res(2) += q_dot(2);
-
+ // q_dot = [ x, y, t ]
+ // w = [ 0, 0, t ]
+ // v = [ x, y, 0 ]
+ // Considering only the 2x2 top left corner:
+ // Sp = [ 0, -1; 1, 0],
+ // if t > 0: S = Sp
+ // else : S = -Sp
+ // S / t = Sp / |t|
+ // S * S = - I2
+ // R = I2 + ( 1 - ct) / |t| * S + ( 1 - st / t ) * S * S
+ // = ( 1 - ct) / |t| * S + st / t * I2
+ //
+ // Ru = exp3 (w)
+ // tu = R * v = (1 - ct) / |t| * S * v + st / t * v
+ //
+ // M0 * Mu = ( R0 * Ru, R0 * tu + t0 )
+
+ Eigen::VectorXd::ConstFixedSegmentReturnType<2>::Type v = vs.segment<2>(idx_v());
+ double ct,st; SINCOS (theta, &st, &ct);
+ const double inv_theta = 1/theta;
+ const double c_coeff = (1.-ct) * inv_theta;
+ const double s_coeff = st * inv_theta;
+ const Vector2 Sp_v (-v[1], v[0]);
+
+ if (t > 0) res.head<2>() += R0 * (s_coeff * v + c_coeff * Sp_v);
+ else res.head<2>() += R0 * (s_coeff * v - c_coeff * Sp_v);
+ res[2] += t;
return res;
}
else
{
- res.head<2>() += R0.topLeftCorner<2,2>()*q_dot.head<2>();
+ res.head<2>() += R0*q_dot.head<2>();
+ res[2] += t;
}
return res;
@@ -433,6 +455,8 @@ namespace se3
else if( u == 1) return q_1;
else
{
+ // TODO This only works if idx_v() == 0
+ assert(idx_v() == 0);
TangentVector_t nu(u*difference(q0, q1));
return integrate(q0, nu);
}
@@ -490,6 +514,14 @@ namespace se3
return q;
}
+ bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.segment<NQ> (idx_q ());
+
+ return q_1.isApprox(q_2, prec);
+ }
+
JointModelDense<NQ, NV> toDense_impl() const
{
return JointModelDense<NQ, NV>(id(),idx_q(),idx_v());
diff --git a/src/multibody/joint/joint-prismatic-unaligned.hpp b/src/multibody/joint/joint-prismatic-unaligned.hpp
index d2e913c8424cb26b682b1e1cbaac4320388752b0..0be8e5522736996632dcec087318f8aeaa5b620d 100644
--- a/src/multibody/joint/joint-prismatic-unaligned.hpp
+++ b/src/multibody/joint/joint-prismatic-unaligned.hpp
@@ -253,6 +253,7 @@ namespace se3
NQ = 1,
NV = 1
};
+ typedef double Scalar;
typedef JointDataPrismaticUnaligned JointDataDerived;
typedef JointModelPrismaticUnaligned JointModelDerived;
typedef ConstraintPrismaticUnaligned Constraint_t;
@@ -320,7 +321,6 @@ namespace se3
using JointModelBase<JointModelPrismaticUnaligned>::idx_v;
using JointModelBase<JointModelPrismaticUnaligned>::setIndexes;
typedef Motion::Vector3 Vector3;
- typedef double Scalar;
JointModelPrismaticUnaligned() : axis(Vector3::Constant(NAN)) {}
JointModelPrismaticUnaligned(Scalar x, Scalar y, Scalar z)
@@ -371,10 +371,9 @@ namespace se3
I -= data.UDinv * data.U.transpose();
}
- ConfigVector_t::Scalar finiteDifferenceIncrement() const
+ Scalar finiteDifferenceIncrement() const
{
using std::sqrt;
- typedef ConfigVector_t::Scalar Scalar;
return sqrt(Eigen::NumTraits<Scalar>::epsilon());
}
@@ -445,6 +444,14 @@ namespace se3
return q;
}
+ bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ const Scalar & q_1 = q1[idx_q()];
+ const Scalar & q_2 = q2[idx_q()];
+
+ return (fabs(q_1 - q_2) < prec);
+ }
+
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
diff --git a/src/multibody/joint/joint-prismatic.hpp b/src/multibody/joint/joint-prismatic.hpp
index 7c87680cf88c1916714a05d301b6a792f390b7a9..10a806addf1085e313137f8611a58e2a669bfab1 100644
--- a/src/multibody/joint/joint-prismatic.hpp
+++ b/src/multibody/joint/joint-prismatic.hpp
@@ -337,6 +337,7 @@ namespace se3
NQ = 1,
NV = 1
};
+ typedef double Scalar;
typedef JointDataPrismatic<axis> JointDataDerived;
typedef JointModelPrismatic<axis> JointModelDerived;
typedef ConstraintPrismatic<axis> Constraint_t;
@@ -396,7 +397,6 @@ namespace se3
using JointModelBase<JointModelPrismatic>::idx_v;
using JointModelBase<JointModelPrismatic>::setIndexes;
typedef Motion::Vector3 Vector3;
- typedef double Scalar;
JointDataDerived createData() const { return JointDataDerived(); }
void calc( JointDataDerived& data,
@@ -427,10 +427,9 @@ namespace se3
I -= data.UDinv * data.U.transpose();
}
- typename ConfigVector_t::Scalar finiteDifferenceIncrement() const
+ Scalar finiteDifferenceIncrement() const
{
using std::sqrt;
- typedef typename ConfigVector_t::Scalar Scalar;
return sqrt(Eigen::NumTraits<Scalar>::epsilon());
}
@@ -500,6 +499,14 @@ namespace se3
return q;
}
+ bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ const Scalar & q_1 = q1[idx_q()];
+ const Scalar & q_2 = q2[idx_q()];
+
+ return (fabs(q_1 - q_2) < prec);
+ }
+
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
diff --git a/src/multibody/joint/joint-revolute-unaligned.hpp b/src/multibody/joint/joint-revolute-unaligned.hpp
index 89e32737dd58ea45875e0a404484fe91abf9ebc3..6dd3817ea8699b29ead71a3fa6a5d8ed9b5a2464 100644
--- a/src/multibody/joint/joint-revolute-unaligned.hpp
+++ b/src/multibody/joint/joint-revolute-unaligned.hpp
@@ -256,7 +256,7 @@ namespace se3
NQ = 1,
NV = 1
};
-
+ typedef double Scalar;
typedef JointDataRevoluteUnaligned JointDataDerived;
typedef JointModelRevoluteUnaligned JointModelDerived;
typedef ConstraintRevoluteUnaligned Constraint_t;
@@ -324,7 +324,6 @@ namespace se3
using JointModelBase<JointModelRevoluteUnaligned>::idx_v;
using JointModelBase<JointModelRevoluteUnaligned>::setIndexes;
typedef Motion::Vector3 Vector3;
- typedef double Scalar;
JointModelRevoluteUnaligned() : axis(Eigen::Vector3d::Constant(NAN)) {}
JointModelRevoluteUnaligned(const double x, const double y, const double z)
@@ -376,10 +375,9 @@ namespace se3
I -= data.UDinv * data.U.transpose();
}
- ConfigVector_t::Scalar finiteDifferenceIncrement() const
+ Scalar finiteDifferenceIncrement() const
{
using std::sqrt;
- typedef ConfigVector_t::Scalar Scalar;
return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
}
@@ -449,6 +447,14 @@ namespace se3
return q;
}
+ bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ const Scalar & q_1 = q1[idx_q()];
+ const Scalar & q_2 = q2[idx_q()];
+
+ return (fabs(q_1 - q_2) < prec);
+ }
+
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
diff --git a/src/multibody/joint/joint-revolute-unbounded.hpp b/src/multibody/joint/joint-revolute-unbounded.hpp
index ec27b7249927b4a3601bee0d426c1d063fac72f7..259ad52583577bb8f4702c6bdc6eb75aa754ffa6 100644
--- a/src/multibody/joint/joint-revolute-unbounded.hpp
+++ b/src/multibody/joint/joint-revolute-unbounded.hpp
@@ -43,7 +43,7 @@ namespace se3
NQ = 2,
NV = 1
};
-
+ typedef double Scalar;
typedef JointDataRevoluteUnbounded<axis> JointDataDerived;
typedef JointModelRevoluteUnbounded<axis> JointModelDerived;
typedef ConstraintRevolute<axis> Constraint_t;
@@ -101,7 +101,6 @@ namespace se3
using JointModelBase<JointModelRevoluteUnbounded>::idx_v;
using JointModelBase<JointModelRevoluteUnbounded>::setIndexes;
typedef Motion::Vector3 Vector3;
- typedef double Scalar;
JointDataDerived createData() const { return JointDataDerived(); }
void calc( JointDataDerived& data,
@@ -140,10 +139,9 @@ namespace se3
I -= data.UDinv * data.U.transpose();
}
- typename ConfigVector_t::Scalar finiteDifferenceIncrement() const
+ Scalar finiteDifferenceIncrement() const
{
using std::sqrt;
- typedef typename ConfigVector_t::Scalar Scalar;
return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
}
@@ -253,6 +251,14 @@ namespace se3
q.segment<NQ>(idx_q()).normalize();
}
+ bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ typename Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
+ typename Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.segment<NQ> (idx_q ());
+
+ return q_1.isApprox(q_2, prec);
+ }
+
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index 21f2dc1e3808bcd049b83016b0955b0da60b8060..f529a728b1d5fb1811d2c933520593c1f50a0547 100644
--- a/src/multibody/joint/joint-revolute.hpp
+++ b/src/multibody/joint/joint-revolute.hpp
@@ -369,7 +369,7 @@ namespace se3
NQ = 1,
NV = 1
};
-
+ typedef double Scalar;
typedef JointDataRevolute<axis> JointDataDerived;
typedef JointModelRevolute<axis> JointModelDerived;
typedef ConstraintRevolute<axis> Constraint_t;
@@ -427,7 +427,6 @@ namespace se3
using JointModelBase<JointModelRevolute>::idx_v;
using JointModelBase<JointModelRevolute>::setIndexes;
typedef Motion::Vector3 Vector3;
- typedef double Scalar;
JointDataDerived createData() const { return JointDataDerived(); }
void calc( JointDataDerived& data,
@@ -461,10 +460,9 @@ namespace se3
I -= data.UDinv * data.U.transpose();
}
- typename ConfigVector_t::Scalar finiteDifferenceIncrement() const
+ Scalar finiteDifferenceIncrement() const
{
using std::sqrt;
- typedef typename ConfigVector_t::Scalar Scalar;
return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
}
@@ -522,6 +520,14 @@ namespace se3
return result;
}
+ bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ const Scalar & q_1 = q1[idx_q()];
+ const Scalar & q_2 = q2[idx_q()];
+
+ return (fabs(q_1 - q_2) < prec);
+ }
+
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return fabs(difference_impl(q0,q1)[0]);
diff --git a/src/multibody/joint/joint-spherical-ZYX.hpp b/src/multibody/joint/joint-spherical-ZYX.hpp
index 2410b5dc09430ae4a6cda678aa8ca99d454e0b8d..f4373834c6c9757e933ae5cf945a645a91f19570 100644
--- a/src/multibody/joint/joint-spherical-ZYX.hpp
+++ b/src/multibody/joint/joint-spherical-ZYX.hpp
@@ -270,6 +270,7 @@ namespace se3
NQ = 3,
NV = 3
};
+ typedef double Scalar;
typedef JointDataSphericalZYX JointDataDerived;
typedef JointModelSphericalZYX JointModelDerived;
typedef JointSphericalZYX::ConstraintRotationalSubspace Constraint_t;
@@ -294,7 +295,6 @@ namespace se3
typedef JointSphericalZYX JointDerived;
SE3_JOINT_TYPEDEF;
- typedef Motion::Scalar Scalar;
typedef Eigen::Matrix<Scalar,6,6> Matrix6;
typedef Eigen::Matrix<Scalar,3,3> Matrix3;
@@ -330,7 +330,6 @@ namespace se3
using JointModelBase<JointModelSphericalZYX>::idx_v;
using JointModelBase<JointModelSphericalZYX>::setIndexes;
typedef Motion::Vector3 Vector3;
- typedef double Scalar;
JointDataDerived createData() const { return JointDataDerived(); }
@@ -398,10 +397,9 @@ namespace se3
I -= data.UDinv * data.U.transpose();
}
- ConfigVector_t::Scalar finiteDifferenceIncrement() const
+ Scalar finiteDifferenceIncrement() const
{
using std::sqrt;
- typedef ConfigVector_t::Scalar Scalar;
return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
}
@@ -465,6 +463,14 @@ namespace se3
ConfigVector_t q;
q << 0, 0, 0;
return q;
+ }
+
+ bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.segment<NQ> (idx_q ());
+
+ return q_1.isApprox(q_2, prec);
}
JointModelDense<NQ,NV> toDense_impl() const
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index 08d6f720fd626c4ddfafd2059eb6bbeedd1db10a..fdbf086c5a3d9e412cf443f081ee93ad368423f4 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -205,6 +205,7 @@ namespace se3
NQ = 4,
NV = 3
};
+ typedef double Scalar;
typedef JointDataSpherical JointDataDerived;
typedef JointModelSpherical JointModelDerived;
typedef ConstraintRotationalSubspace Constraint_t;
@@ -263,7 +264,6 @@ namespace se3
using JointModelBase<JointModelSpherical>::idx_v;
using JointModelBase<JointModelSpherical>::setIndexes;
typedef Motion::Vector3 Vector3;
- typedef double Scalar;
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
JointDataDerived createData() const { return JointDataDerived(); }
@@ -320,10 +320,9 @@ namespace se3
}
}
- ConfigVector_t::Scalar finiteDifferenceIncrement() const
+ Scalar finiteDifferenceIncrement() const
{
using std::sqrt;
- typedef ConfigVector_t::Scalar Scalar;
return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
}
@@ -411,6 +410,19 @@ namespace se3
q.segment<NQ>(idx_q()).normalize();
}
+ bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
+
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.segment<NQ> (idx_q ());
+
+ ConstQuaternionMap_t quat1(q_1.data());
+ ConstQuaternionMap_t quat2(q_2.data());
+
+ return defineSameRotation(quat1,quat2);
+ }
+
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
diff --git a/src/multibody/joint/joint-translation.hpp b/src/multibody/joint/joint-translation.hpp
index 59718e362d5283b5cd68665a4a38f8cb5674b704..944c96927e34b34af467e04d562dee6c94840f5a 100644
--- a/src/multibody/joint/joint-translation.hpp
+++ b/src/multibody/joint/joint-translation.hpp
@@ -219,6 +219,7 @@ namespace se3
NQ = 3,
NV = 3
};
+ typedef double Scalar;
typedef JointDataTranslation JointDataDerived;
typedef JointModelTranslation JointModelDerived;
typedef ConstraintTranslationSubspace Constraint_t;
@@ -278,7 +279,6 @@ namespace se3
using JointModelBase<JointModelTranslation>::idx_v;
using JointModelBase<JointModelTranslation>::setIndexes;
typedef Motion::Vector3 Vector3;
- typedef double Scalar;
JointDataDerived createData() const { return JointDataDerived(); }
@@ -310,10 +310,9 @@ namespace se3
}
}
- ConfigVector_t::Scalar finiteDifferenceIncrement() const
+ Scalar finiteDifferenceIncrement() const
{
using std::sqrt;
- typedef ConfigVector_t::Scalar Scalar;
return sqrt(Eigen::NumTraits<Scalar>::epsilon());
}
@@ -378,6 +377,14 @@ namespace se3
return q;
}
+ bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.segment<NQ> (idx_q ());
+
+ return q_1.isApprox(q_2, prec);
+ }
+
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
diff --git a/src/multibody/joint/joint.hpp b/src/multibody/joint/joint.hpp
index 03892cfea522f2ef7f5a7b23125aba0b53a6af15..84f2637d437996b16e871c3b46baad7acdfb6a2a 100644
--- a/src/multibody/joint/joint.hpp
+++ b/src/multibody/joint/joint.hpp
@@ -36,6 +36,7 @@ namespace se3
NQ = -1, // Dynamic because unknown at compilation
NV = -1
};
+ typedef double Scalar;
typedef JointData JointDataDerived;
typedef JointModel JointModelDerived;
typedef ConstraintXd Constraint_t;
@@ -132,6 +133,9 @@ namespace se3
ConfigVector_t neutralConfiguration_impl() const
{ return ::se3::neutralConfiguration(*this); }
+ bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ { return ::se3::isSameConfiguration(*this, q1, q2);}
+
std::string shortname() const { return ::se3::shortname(*this); }
static std::string classname() { return "JointModel"; }
diff --git a/src/multibody/visitor.hpp b/src/multibody/visitor.hpp
index fb768442d7f2c3c932541fbd2465a8d34f2a0a9a..d0cce5bb57d86d4d30e86f65602b7f0cb5245ad6 100644
--- a/src/multibody/visitor.hpp
+++ b/src/multibody/visitor.hpp
@@ -27,10 +27,13 @@
namespace boost {
namespace fusion {
+
+ // Append the element T at the front of boost fusion vector V.
template<typename T,typename V>
typename result_of::push_front<V const, T>::type
append(T const& t,V const& v) { return push_front(v,t); }
+ // Append the elements T1 and T2 at the front of boost fusion vector V.
template<typename T1,typename T2,typename V>
typename result_of::push_front<typename result_of::push_front<V const, T2>::type const, T1>::type
append2(T1 const& t1,T2 const& t2,V const& v) { return push_front(push_front(v,t2),t1); }
@@ -53,7 +56,7 @@ namespace se3
JointDataVariant& jdataSpec = static_cast<const Visitor*>(this)->jdata;
bf::invoke(&Visitor::template algo<D>,
- bf::append2(jmodel,
+ bf::append2(boost::ref(jmodel),
boost::ref(boost::get<typename D::JointDataDerived>(jdataSpec)),
static_cast<const Visitor*>(this)->args));
}
@@ -74,10 +77,9 @@ namespace se3
template<typename D>
void operator() (const JointModelBase<D> & jmodel) const
{
-
- bf::invoke(&Visitor::template algo<D>,
- bf::append(jmodel,
- static_cast<const Visitor*>(this)->args));
+ bf::invoke(&Visitor::template algo<D>,
+ bf::append(boost::ref(jmodel),
+ static_cast<const Visitor*>(this)->args));
}
template<typename ArgsTmp>
diff --git a/src/spatial/explog.hpp b/src/spatial/explog.hpp
index 7794b146bbbc93fb848965877e764dab6914dd93..af9966690826cef547ea0a624bd287078cb94a23 100644
--- a/src/spatial/explog.hpp
+++ b/src/spatial/explog.hpp
@@ -83,9 +83,10 @@ namespace se3
Scalar t = w.norm();
if (t > 1e-15)
{
- Matrix3 S(alphaSkew(1./t, w));
+ const double inv_t = 1./t;
+ Matrix3 S(alphaSkew(inv_t, w));
double ct,st; SINCOS (t,&st,&ct);
- Matrix3 V((1. - ct)/t * S + (1. - st/t) * S * S);
+ Matrix3 V((1. - ct) * inv_t * S + (1. - st * inv_t) * S * S);
Vector3 p(v + V * v);
return SE3Tpl<_Scalar, _Options>(exp3(w), p);
}
diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt
index a9798562b1fbc6922ca945e5073e6b9248e361f8..329a780b5f26c9a0074a2558fbd37f697b9f746c 100644
--- a/unittest/CMakeLists.txt
+++ b/unittest/CMakeLists.txt
@@ -80,8 +80,12 @@ IF(URDFDOM_FOUND)
IF(BUILD_TESTS_WITH_HPP)
ADD_OPTIONAL_DEPENDENCY("hpp-model-urdf")
IF(HPP_MODEL_URDF_FOUND)
+ ADD_OPTIONAL_DEPENDENCY("romeo_description >= 0.2")
PKG_CONFIG_USE_DEPENDENCY(geom hpp-model-urdf)
ADD_TEST_CFLAGS(geom "-DWITH_HPP_MODEL_URDF")
+ IF(ROMEO_DESCRIPTION_FOUND)
+ ADD_TEST_CFLAGS(geom '-DROMEO_DESCRIPTION_MODEL_DIR=\\\"${ROMEO_DESCRIPTION_DATAROOTDIR}\\\"')
+ ENDIF(ROMEO_DESCRIPTION_FOUND)
ENDIF(HPP_MODEL_URDF_FOUND)
ENDIF(BUILD_TESTS_WITH_HPP)
ENDIF(HPP_FCL_FOUND)
@@ -109,3 +113,4 @@ ADD_UNIT_TEST(joint-configurations eigen3)
ADD_UNIT_TEST(joint eigen3)
ADD_UNIT_TEST(explog eigen3)
ADD_UNIT_TEST(finite-differences eigen3)
+ADD_UNIT_TEST(visitor eigen3)
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index d2eb52f0ffe08f01879a924d282036024bd800b5..3ad8253ac238b897aac57ce94e6f639ff9a40de6 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -247,11 +247,15 @@ BOOST_AUTO_TEST_CASE ( loading_model )
#if defined(WITH_URDFDOM) && defined(WITH_HPP_FCL)
BOOST_AUTO_TEST_CASE (radius)
{
- // Building the model in pinocchio and compute kinematics/geometry for configuration q_pino
- std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
std::vector < std::string > packageDirs;
+#ifdef ROMEO_DESCRIPTION_MODEL_DIR
+ std::string filename = ROMEO_DESCRIPTION_MODEL_DIR"/romeo_description/urdf/romeo_small.urdf";
+ packageDirs.push_back(ROMEO_DESCRIPTION_MODEL_DIR);
+#else
+ std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
packageDirs.push_back(meshDir);
+#endif // ROMEO_DESCRIPTION_MODEL_DIR
se3::Model model;
se3::urdf::buildModel(filename, se3::JointModelFreeFlyer(),model);
@@ -264,7 +268,7 @@ BOOST_AUTO_TEST_CASE (radius)
se3::computeBodyRadius(model, geom, geomData);
BOOST_FOREACH( double radius, geomData.radius) BOOST_CHECK(radius>=0.);
-#ifdef WITH_HPP_MODEL_URDF
+#if defined(WITH_HPP_MODEL_URDF) && defined(ROMEO_DESCRIPTION_MODEL_DIR)
/// ************* HPP ************* ///
/// ********************************* ///
using hpp::model::JointVector_t;
@@ -275,7 +279,7 @@ BOOST_AUTO_TEST_CASE (radius)
hpp::model::HumanoidRobotPtr_t humanoidRobot =
hpp::model::HumanoidRobot::create ("romeo");
loadHumanoidPathPlanerModel(humanoidRobot, "freeflyer",
- "romeo_pinocchio", "romeo",
+ "romeo_description", "romeo_small",
"");
BOOST_CHECK_MESSAGE(model.nq == humanoidRobot->configSize () , "Pinocchio model & HPP model config sizes are not the same ");
@@ -303,7 +307,7 @@ BOOST_AUTO_TEST_CASE (radius)
}
#endif // if defined(WITH_URDFDOM) && defined(WITH_HPP_FCL)
-#ifdef WITH_HPP_MODEL_URDF
+#if defined(WITH_HPP_MODEL_URDF) && defined(ROMEO_DESCRIPTION_MODEL_DIR)
BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
{
typedef se3::Model Model;
@@ -320,10 +324,15 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
/// ********************************* ///
// Building the model in pinocchio and compute kinematics/geometry for configuration q_pino
- std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
std::vector < std::string > packageDirs;
+#ifdef ROMEO_DESCRIPTION_MODEL_DIR
+ std::string filename = ROMEO_DESCRIPTION_MODEL_DIR"/romeo_description/urdf/romeo_small.urdf";
+ packageDirs.push_back(ROMEO_DESCRIPTION_MODEL_DIR);
+#else
+ std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
packageDirs.push_back(meshDir);
+#endif // ROMEO_DESCRIPTION_MODEL_DIR
Model model;
se3::urdf::buildModel(filename, se3::JointModelFreeFlyer(),model);
@@ -358,7 +367,7 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
hpp::model::HumanoidRobotPtr_t humanoidRobot =
hpp::model::HumanoidRobot::create ("romeo");
loadHumanoidPathPlanerModel(humanoidRobot, "freeflyer",
- "romeo_pinocchio", "romeo",
+ "romeo_description", "romeo_small",
"");
BOOST_CHECK_MESSAGE(model.nq == humanoidRobot->configSize () , "Pinocchio model & HPP model config sizes are not the same ");
@@ -425,10 +434,15 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
/// ********************************* ///
// Building the model in pinocchio and compute kinematics/geometry for configuration q_pino
- std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
std::vector < std::string > packageDirs;
+#ifdef ROMEO_DESCRIPTION_MODEL_DIR
+ std::string filename = ROMEO_DESCRIPTION_MODEL_DIR"/romeo_description/urdf/romeo_small.urdf";
+ packageDirs.push_back(ROMEO_DESCRIPTION_MODEL_DIR);
+#else
+ std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
packageDirs.push_back(meshDir);
+#endif // ROMEO_DESCRIPTION_MODEL_DIR
Model model;
se3::urdf::buildModel(filename, se3::JointModelFreeFlyer(),model);
@@ -464,7 +478,7 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
hpp::model::HumanoidRobotPtr_t humanoidRobot =
hpp::model::HumanoidRobot::create ("romeo");
loadHumanoidPathPlanerModel(humanoidRobot, "freeflyer",
- "romeo_pinocchio", "romeo",
+ "romeo_description", "romeo_small",
"");
BOOST_CHECK_MESSAGE(model.nq == humanoidRobot->configSize () , "Pinocchio model & HPP model config sizes are not the same ");
diff --git a/unittest/joint-configurations.cpp b/unittest/joint-configurations.cpp
index 8d09a14b7c52d04af2d3cd74de56d27b551645d9..d305b0ea3a4edf8b1ec2bf25b20c40d480f9d6e2 100644
--- a/unittest/joint-configurations.cpp
+++ b/unittest/joint-configurations.cpp
@@ -102,6 +102,19 @@ struct TestIntegrationJoint
SE3 M1_exp = M0*exp6(v0);
BOOST_CHECK(M1.isApprox(M1_exp));
+
+ qdot *= -1;
+
+ jmodel.calc(jdata,q0,qdot);
+ M0 = jdata.M;
+ v0 = jdata.v;
+
+ q1 = jmodel.integrate(q0,qdot);
+ jmodel.calc(jdata,q1);
+ M1 = jdata.M;
+
+ M1_exp = M0*exp6(v0);
+ BOOST_CHECK(M1.isApprox(M1_exp));
}
};
@@ -175,6 +188,8 @@ struct TestDifferentiationJoint
BOOST_CHECK_MESSAGE( jmodel.integrate(q0, qdot).isApprox(q1), std::string("Error in difference for joint " + jmodel.shortname()));
+ BOOST_CHECK_MESSAGE( jmodel.integrate(q1, -qdot).isApprox(q0), std::string("Error in difference for joint " + jmodel.shortname()));
+
}
void operator()(JointModelBase<JointModelFreeFlyer> & jmodel)
@@ -249,7 +264,7 @@ BOOST_AUTO_TEST_CASE ( integrate_difference_test )
Eigen::VectorXd q1(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
Eigen::VectorXd qdot(Eigen::VectorXd::Random(model.nv));
- BOOST_CHECK_MESSAGE(configurations_are_equals(integrate(model, q0, differentiate(model, q0,q1)), q1), "Integrate (differentiate) - wrong results");
+ BOOST_CHECK_MESSAGE(isSameConfiguration(model, integrate(model, q0, differentiate(model, q0,q1)), q1), "Integrate (differentiate) - wrong results");
BOOST_CHECK_MESSAGE(differentiate(model, q0, integrate(model,q0, qdot)).isApprox(qdot),"differentiate (integrate) - wrong results");
}
diff --git a/unittest/visitor.cpp b/unittest/visitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bd4dfea3739ce54662cbf8cb48273a5d7d474b5d
--- /dev/null
+++ b/unittest/visitor.cpp
@@ -0,0 +1,95 @@
+//
+// Copyright (c) 2015-2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#include <iostream>
+
+#include <pinocchio/multibody/joint/joint.hpp>
+#include <pinocchio/multibody/model.hpp>
+#include "pinocchio/multibody/visitor.hpp"
+
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE ModuleTestVisitor
+
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+
+namespace se3
+{
+
+ struct SimpleVisitor : public se3::fusion::JointVisitor<SimpleVisitor>
+ {
+ typedef boost::fusion::vector<const se3::Model &,
+ se3::Data &,
+ int
+ > ArgsType;
+
+ JOINT_VISITOR_INIT(SimpleVisitor);
+
+ template<typename JointModel>
+ static void algo(const se3::JointModelBase<JointModel> & jmodel,
+ se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
+ const se3::Model & model,
+ se3::Data & data,
+ int jointId);
+ };
+
+ template<typename JointModel>
+ void SimpleVisitor::algo(const se3::JointModelBase<JointModel> & /*jmodel*/,
+ se3::JointDataBase<typename JointModel::JointDataDerived> & /*jdata*/,
+ const se3::Model & /*model*/,
+ se3::Data & /*data*/,
+ int /*dummy*/)
+ { /* --- do nothing --- */ }
+
+ template<>
+ void SimpleVisitor::algo(const se3::JointModelBase<JointModelRevoluteUnaligned> & jmodel,
+ se3::JointDataBase<JointDataRevoluteUnaligned> & /*jdata*/,
+ const se3::Model & /*model*/,
+ se3::Data & /*data*/,
+ int /*dummy*/)
+ {
+ BOOST_CHECK( jmodel.shortname() == JointModelRevoluteUnaligned::classname() );
+ Eigen::Vector3d axis = jmodel.derived().axis;
+ Eigen::Vector3d axis_z; axis_z << 0,0,1;
+
+ BOOST_CHECK ( axis == axis_z );
+ }
+
+} // namespace se3
+
+/* Validates the access to memory stored in joint models, by using the class
+ * joint model revolute unaligned.
+ */
+
+BOOST_AUTO_TEST_SUITE ( TestVisitor )
+
+BOOST_AUTO_TEST_CASE ( test_runal )
+{
+ using namespace se3;
+
+ se3::Model model;
+ model.addJoint(0,se3::JointModelRevoluteUnaligned(0,0,1),se3::SE3::Random());
+ Data data(model);
+
+ for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoint;++i )
+ {
+ SimpleVisitor::run(model.joints[i],data.joints[i],
+ SimpleVisitor::ArgsType(model,data,i));
+ }
+}
+
+BOOST_AUTO_TEST_SUITE_END ()