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 ...@@ -112,6 +112,20 @@ namespace se3
*/ */
inline void normalize(const Model & model, inline void normalize(const Model & model,
Eigen::VectorXd & q); 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 } // namespace se3
/* --- Details -------------------------------------------------------------------- */ /* --- Details -------------------------------------------------------------------- */
...@@ -332,6 +346,40 @@ namespace se3 ...@@ -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 } // namespace se3
#endif // ifndef __se3_joint_configuration_hpp__ #endif // ifndef __se3_joint_configuration_hpp__
......
...@@ -68,6 +68,7 @@ namespace se3 ...@@ -68,6 +68,7 @@ namespace se3
#define SE3_JOINT_TYPEDEF_ARG(prefix) \ #define SE3_JOINT_TYPEDEF_ARG(prefix) \
typedef int Index; \ typedef int Index; \
typedef prefix traits<JointDerived>::Scalar Scalar; \
typedef prefix traits<JointDerived>::JointDataDerived JointDataDerived; \ typedef prefix traits<JointDerived>::JointDataDerived JointDataDerived; \
typedef prefix traits<JointDerived>::JointModelDerived JointModelDerived; \ typedef prefix traits<JointDerived>::JointModelDerived JointModelDerived; \
typedef prefix traits<JointDerived>::Constraint_t Constraint_t; \ typedef prefix traits<JointDerived>::Constraint_t Constraint_t; \
...@@ -92,6 +93,7 @@ namespace se3 ...@@ -92,6 +93,7 @@ namespace se3
#define SE3_JOINT_TYPEDEF_NOARG() \ #define SE3_JOINT_TYPEDEF_NOARG() \
typedef int Index; \ typedef int Index; \
typedef traits<JointDerived>::Scalar Scalar; \
typedef traits<JointDerived>::JointDataDerived JointDataDerived; \ typedef traits<JointDerived>::JointDataDerived JointDataDerived; \
typedef traits<JointDerived>::JointModelDerived JointModelDerived; \ typedef traits<JointDerived>::JointModelDerived JointModelDerived; \
typedef traits<JointDerived>::Constraint_t Constraint_t; \ typedef traits<JointDerived>::Constraint_t Constraint_t; \
...@@ -111,6 +113,7 @@ namespace se3 ...@@ -111,6 +113,7 @@ namespace se3
#define SE3_JOINT_TYPEDEF_ARG(prefix) \ #define SE3_JOINT_TYPEDEF_ARG(prefix) \
typedef int Index; \ typedef int Index; \
typedef prefix traits<JointDerived>::Scalar Scalar;
typedef prefix traits<JointDerived>::JointDataDerived JointDataDerived; \ typedef prefix traits<JointDerived>::JointDataDerived JointDataDerived; \
typedef prefix traits<JointDerived>::JointModelDerived JointModelDerived; \ typedef prefix traits<JointDerived>::JointModelDerived JointModelDerived; \
typedef prefix traits<JointDerived>::Constraint_t Constraint_t; \ typedef prefix traits<JointDerived>::Constraint_t Constraint_t; \
...@@ -135,6 +138,7 @@ namespace se3 ...@@ -135,6 +138,7 @@ namespace se3
#define SE3_JOINT_TYPEDEF_ARG() \ #define SE3_JOINT_TYPEDEF_ARG() \
typedef int Index; \ typedef int Index; \
typedef typename traits<JointDerived>::Scalar Scalar; \
typedef typename traits<JointDerived>::JointDataDerived JointDataDerived; \ typedef typename traits<JointDerived>::JointDataDerived JointDataDerived; \
typedef typename traits<JointDerived>::JointModelDerived JointModelDerived; \ typedef typename traits<JointDerived>::JointModelDerived JointModelDerived; \
typedef typename traits<JointDerived>::Constraint_t Constraint_t; \ typedef typename traits<JointDerived>::Constraint_t Constraint_t; \
...@@ -356,6 +360,15 @@ namespace se3 ...@@ -356,6 +360,15 @@ namespace se3
*/ */
void normalize_impl(Eigen::VectorXd &) const { } 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. 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_q; // Index of the joint configuration in the joint configuration vector.
int i_v; // Index of the joint velocity in the joint velocity vector. int i_v; // Index of the joint velocity in the joint velocity vector.
......
...@@ -173,6 +173,20 @@ namespace se3 ...@@ -173,6 +173,20 @@ namespace se3
inline void normalize(const JointModelVariant & jmodel, inline void normalize(const JointModelVariant & jmodel,
Eigen::VectorXd & q); 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 * @brief Visit a JointModelVariant through JointNvVisitor to get the dimension of
* the joint tangent space * the joint tangent space
......
...@@ -265,6 +265,31 @@ namespace se3 ...@@ -265,6 +265,31 @@ namespace se3
JointNormalizeVisitor::run(jmodel, q); 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 * @brief JointDifferenceVisitor visitor
......
...@@ -30,6 +30,7 @@ namespace se3 ...@@ -30,6 +30,7 @@ namespace se3
NQ = _NQ, // pb NQ = _NQ, // pb
NV = _NV NV = _NV
}; };
typedef double Scalar;
typedef JointDataDense<_NQ, _NV> JointDataDerived; typedef JointDataDense<_NQ, _NV> JointDataDerived;
typedef JointModelDense<_NQ, _NV> JointModelDerived; typedef JointModelDense<_NQ, _NV> JointModelDerived;
typedef ConstraintXd Constraint_t; typedef ConstraintXd Constraint_t;
...@@ -202,6 +203,11 @@ namespace se3 ...@@ -202,6 +203,11 @@ namespace se3
assert(false && "JointModelDense is read-only, should not perform any calc"); 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() {}
JointModelDense(JointIndex idx, int idx_q, int idx_v) JointModelDense(JointIndex idx, int idx_q, int idx_v)
{ {
......
...@@ -142,6 +142,7 @@ namespace se3 ...@@ -142,6 +142,7 @@ namespace se3
NQ = 7, NQ = 7,
NV = 6 NV = 6
}; };
typedef double Scalar;
typedef JointDataFreeFlyer JointDataDerived; typedef JointDataFreeFlyer JointDataDerived;
typedef JointModelFreeFlyer JointModelDerived; typedef JointModelFreeFlyer JointModelDerived;
typedef ConstraintIdentity Constraint_t; typedef ConstraintIdentity Constraint_t;
...@@ -200,7 +201,6 @@ namespace se3 ...@@ -200,7 +201,6 @@ namespace se3
using JointModelBase<JointModelFreeFlyer>::idx_v; using JointModelBase<JointModelFreeFlyer>::idx_v;
using JointModelBase<JointModelFreeFlyer>::setIndexes; using JointModelBase<JointModelFreeFlyer>::setIndexes;
typedef Motion::Vector3 Vector3; typedef Motion::Vector3 Vector3;
typedef double Scalar;
JointDataDerived createData() const { return JointDataDerived(); } JointDataDerived createData() const { return JointDataDerived(); }
...@@ -254,10 +254,9 @@ namespace se3 ...@@ -254,10 +254,9 @@ namespace se3
I.setZero(); I.setZero();
} }
ConfigVector_t::Scalar finiteDifferenceIncrement() const Scalar finiteDifferenceIncrement() const
{ {
using std::sqrt; using std::sqrt;
typedef ConfigVector_t::Scalar Scalar;
return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon())); return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
} }
...@@ -362,6 +361,19 @@ namespace se3 ...@@ -362,6 +361,19 @@ namespace se3
q.segment<4>(idx_q()+3).normalize(); 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 JointModelDense<NQ,NV> toDense_impl() const
{ {
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v()); return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
......
...@@ -273,6 +273,7 @@ namespace se3 ...@@ -273,6 +273,7 @@ namespace se3
NQ = 3, NQ = 3,
NV = 3 NV = 3
}; };
typedef double Scalar;
typedef JointDataPlanar JointDataDerived; typedef JointDataPlanar JointDataDerived;
typedef JointModelPlanar JointModelDerived; typedef JointModelPlanar JointModelDerived;
typedef ConstraintPlanar Constraint_t; typedef ConstraintPlanar Constraint_t;
...@@ -490,6 +491,14 @@ namespace se3 ...@@ -490,6 +491,14 @@ namespace se3
return q; 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 JointModelDense<NQ, NV> toDense_impl() const
{ {
return JointModelDense<NQ, NV>(id(),idx_q(),idx_v()); return JointModelDense<NQ, NV>(id(),idx_q(),idx_v());
......
...@@ -253,6 +253,7 @@ namespace se3 ...@@ -253,6 +253,7 @@ namespace se3
NQ = 1, NQ = 1,
NV = 1 NV = 1
}; };
typedef double Scalar;
typedef JointDataPrismaticUnaligned JointDataDerived; typedef JointDataPrismaticUnaligned JointDataDerived;
typedef JointModelPrismaticUnaligned JointModelDerived; typedef JointModelPrismaticUnaligned JointModelDerived;
typedef ConstraintPrismaticUnaligned Constraint_t; typedef ConstraintPrismaticUnaligned Constraint_t;
...@@ -320,7 +321,6 @@ namespace se3 ...@@ -320,7 +321,6 @@ namespace se3
using JointModelBase<JointModelPrismaticUnaligned>::idx_v; using JointModelBase<JointModelPrismaticUnaligned>::idx_v;
using JointModelBase<JointModelPrismaticUnaligned>::setIndexes; using JointModelBase<JointModelPrismaticUnaligned>::setIndexes;
typedef Motion::Vector3 Vector3; typedef Motion::Vector3 Vector3;
typedef double Scalar;
JointModelPrismaticUnaligned() : axis(Vector3::Constant(NAN)) {} JointModelPrismaticUnaligned() : axis(Vector3::Constant(NAN)) {}
JointModelPrismaticUnaligned(Scalar x, Scalar y, Scalar z) JointModelPrismaticUnaligned(Scalar x, Scalar y, Scalar z)
...@@ -371,10 +371,9 @@ namespace se3 ...@@ -371,10 +371,9 @@ namespace se3
I -= data.UDinv * data.U.transpose(); I -= data.UDinv * data.U.transpose();
} }
ConfigVector_t::Scalar finiteDifferenceIncrement() const Scalar finiteDifferenceIncrement() const
{ {
using std::sqrt; using std::sqrt;
typedef ConfigVector_t::Scalar Scalar;
return sqrt(Eigen::NumTraits<Scalar>::epsilon()); return sqrt(Eigen::NumTraits<Scalar>::epsilon());
} }
...@@ -445,6 +444,14 @@ namespace se3 ...@@ -445,6 +444,14 @@ namespace se3
return q; 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 JointModelDense<NQ,NV> toDense_impl() const
{ {
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v()); return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
......
...@@ -337,6 +337,7 @@ namespace se3 ...@@ -337,6 +337,7 @@ namespace se3
NQ = 1, NQ = 1,
NV = 1 NV = 1
}; };
typedef double Scalar;
typedef JointDataPrismatic<axis> JointDataDerived; typedef JointDataPrismatic<axis> JointDataDerived;
typedef JointModelPrismatic<axis> JointModelDerived; typedef JointModelPrismatic<axis> JointModelDerived;
typedef ConstraintPrismatic<axis> Constraint_t; typedef ConstraintPrismatic<axis> Constraint_t;
...@@ -396,7 +397,6 @@ namespace se3 ...@@ -396,7 +397,6 @@ namespace se3
using JointModelBase<JointModelPrismatic>::idx_v; using JointModelBase<JointModelPrismatic>::idx_v;
using JointModelBase<JointModelPrismatic>::setIndexes; using JointModelBase<JointModelPrismatic>::setIndexes;
typedef Motion::Vector3 Vector3; typedef Motion::Vector3 Vector3;
typedef double Scalar;
JointDataDerived createData() const { return JointDataDerived(); } JointDataDerived createData() const { return JointDataDerived(); }
void calc( JointDataDerived& data, void calc( JointDataDerived& data,
...@@ -427,10 +427,9 @@ namespace se3 ...@@ -427,10 +427,9 @@ namespace se3
I -= data.UDinv * data.U.transpose(); I -= data.UDinv * data.U.transpose();
} }
typename ConfigVector_t::Scalar finiteDifferenceIncrement() const Scalar finiteDifferenceIncrement() const
{ {
using std::sqrt; using std::sqrt;
typedef typename ConfigVector_t::Scalar Scalar;
return sqrt(Eigen::NumTraits<Scalar>::epsilon()); return sqrt(Eigen::NumTraits<Scalar>::epsilon());
} }
...@@ -500,6 +499,14 @@ namespace se3 ...@@ -500,6 +499,14 @@ namespace se3
return q; 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 JointModelDense<NQ,NV> toDense_impl() const
{ {
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v()); return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
......
...@@ -256,7 +256,7 @@ namespace se3 ...@@ -256,7 +256,7 @@ namespace se3
NQ = 1, NQ = 1,
NV = 1 NV = 1
}; };
typedef double Scalar;
typedef JointDataRevoluteUnaligned JointDataDerived; typedef JointDataRevoluteUnaligned JointDataDerived;
typedef JointModelRevoluteUnaligned JointModelDerived; typedef JointModelRevoluteUnaligned JointModelDerived;
typedef ConstraintRevoluteUnaligned Constraint_t; typedef ConstraintRevoluteUnaligned Constraint_t;
...@@ -324,7 +324,6 @@ namespace se3 ...@@ -324,7 +324,6 @@ namespace se3
using JointModelBase<JointModelRevoluteUnaligned>::idx_v; using JointModelBase<JointModelRevoluteUnaligned>::idx_v;
using JointModelBase<JointModelRevoluteUnaligned>::setIndexes; using JointModelBase<JointModelRevoluteUnaligned>::setIndexes;
typedef Motion::Vector3 Vector3; typedef Motion::Vector3 Vector3;
typedef double Scalar;
JointModelRevoluteUnaligned() : axis(Eigen::Vector3d::Constant(NAN)) {} JointModelRevoluteUnaligned() : axis(Eigen::Vector3d::Constant(NAN)) {}
JointModelRevoluteUnaligned(const double x, const double y, const double z) JointModelRevoluteUnaligned(const double x, const double y, const double z)
...@@ -376,10 +375,9 @@ namespace se3 ...@@ -376,10 +375,9 @@ namespace se3
I -= data.UDinv * data.U.transpose(); I -= data.UDinv * data.U.transpose();
} }
ConfigVector_t::Scalar finiteDifferenceIncrement() const Scalar finiteDifferenceIncrement() const
{ {
using std::sqrt; using std::sqrt;
typedef ConfigVector_t::Scalar Scalar;
return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon())); return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
} }
...@@ -449,6 +447,14 @@ namespace se3 ...@@ -449,6 +447,14 @@ namespace se3
return q; 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 JointModelDense<NQ,NV> toDense_impl() const
{ {
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v()); return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
......
...@@ -42,7 +42,7 @@ namespace se3 ...@@ -42,7 +42,7 @@ namespace se3
NQ = 2, NQ = 2,
NV = 1 NV = 1
}; };
typedef double Scalar;
typedef JointDataRevoluteUnbounded<axis> JointDataDerived; typedef JointDataRevoluteUnbounded<axis> JointDataDerived;
typedef JointModelRevoluteUnbounded<axis> JointModelDerived; typedef JointModelRevoluteUnbounded<axis> JointModelDerived;
typedef ConstraintRevolute<axis> Constraint_t; typedef ConstraintRevolute<axis> Constraint_t;
...@@ -100,7 +100,6 @@ namespace se3 ...@@ -100,7 +100,6 @@ namespace se3
using JointModelBase<JointModelRevoluteUnbounded>::idx_v; using JointModelBase<JointModelRevoluteUnbounded>::idx_v;
using JointModelBase<JointModelRevoluteUnbounded>::setIndexes; using JointModelBase<JointModelRevoluteUnbounded>::setIndexes;
typedef Motion::Vector3 Vector3; typedef Motion::Vector3 Vector3;
typedef double Scalar;
JointDataDerived createData() const { return JointDataDerived(); } JointDataDerived createData() const { return JointDataDerived(); }
void calc( JointDataDerived& data, void calc( JointDataDerived& data,
...@@ -139,10 +138,9 @@ namespace se3 ...@@ -139,10 +138,9 @@ namespace se3
I -= data.UDinv * data.U.transpose(); I -= data.UDinv * data.U.transpose();
} }
typename ConfigVector_t::Scalar finiteDifferenceIncrement() const Scalar finiteDifferenceIncrement() const
{ {
using std::sqrt; using std::sqrt;
typedef typename ConfigVector_t::Scalar Scalar;
return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon())); return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
} }
...@@ -251,6 +249,14 @@ namespace se3 ...@@ -251,6 +249,14 @@ namespace se3
q.segment<NQ>(idx_q()).normalize(); 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 JointModelDense<NQ,NV> toDense_impl() const
{ {
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v()); return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
......
...@@ -369,7 +369,7 @@ namespace se3 ...@@ -369,7 +369,7 @@ namespace se3
NQ = 1,