Verified Commit 517c7e60 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

mutibody: add isEqual methods for Joint{Model,Data}Tpl

parent 20ab2696
......@@ -174,6 +174,18 @@ namespace pinocchio
typename CastType< NewScalar,JointModelTpl<Scalar,Options,JointCollectionTpl> >::type
cast_joint(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel);
/**
* @brief Visit a JointModelTpl<Scalar,...> to compare it to JointModelDerived
*
* @param[in] jmodel_generic The generic joint model containing a variant.
* @param[in] jmodel The other joint model for the comparison.
*
* @return True if the two joint models are equal.
*/
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
bool isEqual(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic,
const JointModelBase<JointModelDerived> & jmodel);
//
// Visitors on JointDatas
......@@ -263,6 +275,18 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options>
udinv_inertia(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata);
/**
* @brief Visit a JointDataTpl<Scalar,...> to compare it to another JointData
*
* @param[in] jdata_generic The generic joint data containing a variant.
* @param[in] jdata The other joint data for the comparison.
*
* @return True if the two joints data are equal.
*/
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived>
bool isEqual(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic,
const JointDataBase<JointDataDerived> & jmodel);
} // namespace pinocchio
......
......@@ -301,6 +301,29 @@ namespace pinocchio
return Algo::run(jmodel);
}
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
struct JointModelComparisonOperatorVisitor
: fusion::JointUnaryVisitorBase< JointModelComparisonOperatorVisitor<Scalar,Options,JointCollectionTpl,JointModelDerived>,bool>
{
typedef boost::fusion::vector<const JointModelDerived &> ArgsType;
template<typename JointModel>
static bool algo(const JointModelBase<JointModel> & jmodel_lhs,
const JointModelDerived & jmodel_rhs)
{
return jmodel_lhs == jmodel_rhs;
}
};
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
bool isEqual(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic,
const JointModelBase<JointModelDerived> & jmodel)
{
typedef JointModelComparisonOperatorVisitor<Scalar,Options,JointCollectionTpl,JointModelDerived> Algo;
return Algo::run(jmodel_generic,typename Algo::ArgsType(boost::ref(jmodel.derived())));
}
//
// Visitors on JointDatas
//
......@@ -520,6 +543,30 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline std::string shortname(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata)
{ return JointDataShortnameVisitor::run(jdata);}
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived>
struct JointDataComparisonOperatorVisitor
: fusion::JointUnaryVisitorBase< JointDataComparisonOperatorVisitor<Scalar,Options,JointCollectionTpl,JointDataDerived>,bool>
{
typedef boost::fusion::vector<const JointDataDerived &> ArgsType;
template<typename JointData>
static bool algo(const JointDataBase<JointModel> & jdata_lhs,
const JointDataDerived & jdata_rhs)
{
return jdata_lhs == jdata_rhs;
}
};
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived>
bool isEqual(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata_generic,
const JointDataBase<JointDataDerived> & jdata)
{
typedef JointDataComparisonOperatorVisitor<Scalar,Options,JointCollectionTpl,JointDataDerived> Algo;
return Algo::run(jdata_generic,typename Algo::ArgsType(boost::ref(jdata.derived())));
}
/// @endcond
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment