Commit 9856f2bb by Valenza Florian

### 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 { typedef boost::fusion::vector ArgsType; JOINT_MODEL_VISITOR_INIT(IsSameConfigurationStep); template static void algo(const se3::JointModelBase & 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::Scalar Scalar; \ typedef prefix traits::JointDataDerived JointDataDerived; \ typedef prefix traits::JointModelDerived JointModelDerived; \ typedef prefix traits::Constraint_t Constraint_t; \ ... ... @@ -92,6 +93,7 @@ namespace se3 #define SE3_JOINT_TYPEDEF_NOARG() \ typedef int Index; \ typedef traits::Scalar Scalar; \ typedef traits::JointDataDerived JointDataDerived; \ typedef traits::JointModelDerived JointModelDerived; \ typedef traits::Constraint_t Constraint_t; \ ... ... @@ -111,6 +113,7 @@ namespace se3 #define SE3_JOINT_TYPEDEF_ARG(prefix) \ typedef int Index; \ typedef prefix traits::Scalar Scalar; typedef prefix traits::JointDataDerived JointDataDerived; \ typedef prefix traits::JointModelDerived JointModelDerived; \ typedef prefix traits::Constraint_t Constraint_t; \ ... ... @@ -135,6 +138,7 @@ namespace se3 #define SE3_JOINT_TYPEDEF_ARG() \ typedef int Index; \ typedef typename traits::Scalar Scalar; \ typedef typename traits::JointDataDerived JointDataDerived; \ typedef typename traits::JointModelDerived JointModelDerived; \ typedef typename traits::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::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 { public: const Eigen::VectorXd & q1; const Eigen::VectorXd & q2; JointIsSameConfigurationVisitor(const Eigen::VectorXd & q1,const Eigen::VectorXd & q2) : q1(q1), q2(q2) {} template bool operator()(const JointModelBase & 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::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::idx_v; using JointModelBase::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::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::dummy_precision()) const { typedef Eigen::Map ConstQuaternionMap_t; Eigen::VectorXd::ConstFixedSegmentReturnType::Type & q_1 = q1.segment (idx_q ()); Eigen::VectorXd::ConstFixedSegmentReturnType::Type & q_2 = q2.segment (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 toDense_impl() const { return JointModelDense(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::dummy_precision()) const { Eigen::VectorXd::ConstFixedSegmentReturnType::Type & q_1 = q1.segment (idx_q ()); Eigen::VectorXd::ConstFixedSegmentReturnType::Type & q_2 = q2.segment (idx_q ()); return q_1.isApprox(q_2, prec); } JointModelDense toDense_impl() const { return JointModelDense(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::idx_v; using JointModelBase::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::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::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 toDense_impl() const { return JointModelDense(id(),idx_q(),idx_v()); ... ...
 ... ... @@ -337,6 +337,7 @@ namespace se3 NQ = 1, NV = 1 }; typedef double Scalar; typedef JointDataPrismatic JointDataDerived; typedef JointModelPrismatic JointModelDerived; typedef ConstraintPrismatic Constraint_t; ... ... @@ -396,7 +397,6 @@ namespace se3 using JointModelBase::idx_v; using JointModelBase::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::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::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 toDense_impl() const { return JointModelDense(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::idx_v; using JointModelBase::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::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::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 toDense_impl() const { return JointModelDense(id(),idx_q(),idx_v()); ... ...
 ... ... @@ -42,7 +42,7 @@ namespace se3 NQ = 2, NV = 1 }; typedef double Scalar; typedef JointDataRevoluteUnbounded JointDataDerived; typedef JointModelRevoluteUnbounded JointModelDerived; typedef ConstraintRevolute Constraint_t; ... ... @@ -100,7 +100,6 @@ namespace se3 using JointModelBase::idx_v; using JointModelBase::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::epsilon())); } ... ... @@ -251,6 +249,14 @@ namespace se3 q.segment(idx_q()).normalize(); } bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits::dummy_precision()) const { typename Eigen::VectorXd::ConstFixedSegmentReturnType::Type & q_1 = q1.segment (idx_q ()); typename Eigen::VectorXd::ConstFixedSegmentReturnType::Type & q_2 = q2.segment (idx_q ()); return q_1.isApprox(q_2, prec); } JointModelDense toDense_impl() const { return JointModelDense(id(),idx_q(),idx_v()); ... ...
 ... ... @@ -369,7 +369,7 @@ namespace se3 NQ = 1, NV = 1 }; typedef double Scalar; typedef JointDataRevolute JointDataDerived; typedef JointModelRevolute JointModelDerived; typedef ConstraintRevolute Constraint_t; ... ... @@ -427,7 +427,6 @@ namespace se3 using JointModelBase::idx_v; using JointModelBase::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::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::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 Matrix6; typedef Eigen::Matrix Matrix3; ... ... @@ -330,7 +330,6 @@ namespace se3 using JointModelBase::idx_v; using JointModelBase::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::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::dummy_precision()) const { Eigen::VectorXd::ConstFixedSegmentReturnType::Type & q_1 = q1.segment (idx_q ()); Eigen::VectorXd::ConstFixedSegmentReturnType::Type & q_2 = q2.segment (idx_q ()); return q_1.isApprox(q_2, prec); } JointModelDense 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::idx_v; using JointModelBase::setIndexes; typedef Motion::Vector3 Vector3; typedef double Scalar; typedef Eigen::Map 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::epsilon())); } ... ... @@ -410,6 +409,19 @@ namespace se3 q.segment(idx_q()).normalize(); } bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & = Eigen::NumTraits::dummy_precision()) const { typedef Eigen::Map ConstQuaternionMap_t; Eigen::VectorXd::ConstFixedSegmentReturnType::Type & q_1 = q1.segment (idx_q ()); Eigen::VectorXd::ConstFixedSegmentReturnType::Type & q_2 = q2.segment (idx_q ()); ConstQuaternionMap_t quat1(q_1.data()); ConstQuaternionMap_t quat2(q_2.data()); return defineSameRotation(quat1,quat2); } JointModelDense toDense_impl() const { return JointModelDense(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::idx_v; using JointModelBase::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::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::dummy_precision()) const { Eigen::VectorXd::ConstFixedSegmentReturnType::Type & q_1 = q1.segment (idx_q ()); Eigen::VectorXd::ConstFixedSegmentReturnType::Type & q_2 = q2.s