Commit 9856f2bb authored by Valenza Florian's avatar Valenza Florian
Browse files

Added methods in joints and algorithm working on Model to determine if two...

Added methods in joints and algorithm working on Model to determine if two configurations are equivalent
parent 3f034aa1
......@@ -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__
......
......@@ -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; \
......@@ -356,6 +360,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.
......
......@@ -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
......
......@@ -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
......
......@@ -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)
{
......
......@@ -142,6 +142,7 @@ namespace se3
NQ = 7,
NV = 6
};
typedef double Scalar;
typedef JointDataFreeFlyer JointDataDerived;
typedef JointModelFreeFlyer JointModelDerived;
typedef ConstraintIdentity Constraint_t;
......@@ -200,7 +201,6 @@ namespace se3
using JointModelBase<JointModelFreeFlyer>::idx_v;
using JointModelBase<JointModelFreeFlyer>::setIndexes;
typedef Motion::Vector3 Vector3;
typedef double Scalar;
JointDataDerived createData() const { return JointDataDerived(); }
......@@ -254,10 +254,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()));
}
......@@ -362,6 +361,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());
......
......@@ -273,6 +273,7 @@ namespace se3
NQ = 3,
NV = 3
};
typedef double Scalar;
typedef JointDataPlanar JointDataDerived;
typedef JointModelPlanar JointModelDerived;
typedef ConstraintPlanar Constraint_t;
......@@ -490,6 +491,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());
......
......@@ -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());
......
......@@ -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());
......
......@@ -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());
......
......@@ -42,7 +42,7 @@ namespace se3
NQ = 2,
NV = 1
};
typedef double Scalar;
typedef JointDataRevoluteUnbounded<axis> JointDataDerived;
typedef JointModelRevoluteUnbounded<axis> JointModelDerived;
typedef ConstraintRevolute<axis> Constraint_t;
......@@ -100,7 +100,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,
......@@ -139,10 +138,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()));
}
......@@ -251,6 +249,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());
......
......@@ -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]);
......
......@@ -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
......
......@@ -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()));
}
......@@ -410,6 +409,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());
......
......@@ -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.s