Commit 943c4c75 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[LieGroup] Fix equality operator.

parent 8656c6b0
......@@ -390,6 +390,9 @@ namespace pinocchio
bool isEqual_impl (const CartesianProductOperationVariantTpl& other) const;
template <typename LieGroup1, typename LieGroup2>
bool isEqual(const CartesianProductOperation<LieGroup1, LieGroup2> & other) const;
protected:
std::vector<LieGroupGeneric> liegroups;
......
......@@ -82,6 +82,20 @@ isEqual_impl (const CartesianProductOperationVariantTpl& other) const
return true;
}
template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <typename LieGroup1, typename LieGroup2>
bool CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
isEqual(const CartesianProductOperation<LieGroup1, LieGroup2> & other) const
{
if (liegroups.size() != 2)
return false;
if (liegroups[0].isDifferent_impl(LieGroupGeneric(other.lg1)))
return false;
if (liegroups[1].isDifferent_impl(LieGroupGeneric(other.lg2)))
return false;
return true;
}
} // namespace pinocchio
#endif // ifndef __pinocchio_cartesian_product_variant_hxx__
......@@ -252,6 +252,11 @@ namespace pinocchio
&& lg2.isSameConfiguration(Q2(q0), Q2(q1), prec);
}
bool isEqual_impl (const CartesianProductOperation& other) const
{
return lg1 == other.lg1 && lg2 == other.lg2;
}
LieGroup1 lg1;
LieGroup2 lg2;
......
......@@ -51,11 +51,21 @@ namespace pinocchio
bool isEqual_impl (const LieGroupGenericTpl& other) const
{
return boost::apply_visitor(visitor::LieGroupEqual(), toVariant(), other.toVariant());
return boost::apply_visitor(visitor::LieGroupEqual<Scalar, Options>(), toVariant(), other.toVariant());
}
int nq() const { return ::pinocchio::nq(*this); }
int nv() const { return ::pinocchio::nv(*this); }
bool operator== (const LieGroupGenericTpl& other) const
{
return isEqual_impl(other);
}
bool operator!= (const LieGroupGenericTpl& other) const
{
return this->isDifferent_impl(other);
}
};
}
......
......@@ -6,6 +6,7 @@
#define __pinocchio_lie_group_variant_visitor_hxx__
#include "pinocchio/multibody/liegroup/liegroup-base.hpp"
#include "pinocchio/multibody/liegroup/cartesian-product-variant.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include <string>
......@@ -40,6 +41,7 @@ namespace pinocchio
}
};
template<typename Scalar, int Options>
struct LieGroupEqual : public boost::static_visitor<bool>
{
template <typename T, typename U>
......@@ -56,6 +58,45 @@ namespace pinocchio
{
return lhs == rhs;
}
/// \name Vector space comparison
/// \{
bool operator() (const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & lhs,
const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & rhs ) const
{
return lhs == rhs;
}
template <int Dim>
bool operator() (const VectorSpaceOperationTpl<Dim, Scalar, Options> & lhs,
const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & rhs ) const
{
return lhs.nq() == rhs.nq();
}
template <int Dim>
bool operator() (const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & lhs,
const VectorSpaceOperationTpl<Dim, Scalar, Options> & rhs ) const
{
return lhs.nq() == rhs.nq();
}
/// \}
template <typename LieGroup1, typename LieGroup2,
template<typename,int> class LieGroupCollectionTpl>
bool operator() (const CartesianProductOperation<LieGroup1, LieGroup2> & lhs,
const CartesianProductOperationVariantTpl<Scalar, Options, LieGroupCollectionTpl> & rhs ) const
{
return rhs.isEqual(lhs);
}
template <typename LieGroup1, typename LieGroup2,
template<typename,int> class LieGroupCollectionTpl>
bool operator() (const CartesianProductOperationVariantTpl<Scalar, Options, LieGroupCollectionTpl> & lhs,
const CartesianProductOperation<LieGroup1, LieGroup2> & rhs) const
{
return lhs.isEqual(rhs);
}
};
}
......
......@@ -27,6 +27,19 @@ using namespace pinocchio;
#define VERBOSE false
#define IFVERBOSE if(VERBOSE)
namespace pinocchio {
template<typename Derived>
std::ostream& operator<< (std::ostream& os, const LieGroupBase<Derived>& lg)
{
return os << lg.name();
}
template<typename LieGroupCollection>
std::ostream& operator<< (std::ostream& os, const LieGroupGenericTpl<LieGroupCollection>& lg)
{
return os << lg.name();
}
} // namespace pinocchio
template <typename T>
void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &)
{
......@@ -700,4 +713,29 @@ BOOST_AUTO_TEST_CASE(test_liegroup_variant)
boost::mpl::for_each<LieGroupCollectionDefault::LieGroupVariant::types>(TestLieGroupVariantVisitor<LieGroupCollectionDefault>());
}
template<typename Lg1, typename Lg2>
void test_liegroup_variant_equal(Lg1 lg1, Lg2 lg2)
{
typedef LieGroupGenericTpl<LieGroupCollectionDefault> LieGroupGeneric;
BOOST_CHECK_EQUAL(LieGroupGeneric(lg1), LieGroupGeneric(lg2));
}
template<typename Lg1, typename Lg2>
void test_liegroup_variant_not_equal(Lg1 lg1, Lg2 lg2)
{
typedef LieGroupGenericTpl<LieGroupCollectionDefault> LieGroupGeneric;
BOOST_CHECK_PREDICATE( std::not_equal_to<LieGroupGeneric>(),
(LieGroupGeneric(lg1))(LieGroupGeneric(lg2)) );
}
BOOST_AUTO_TEST_CASE(test_liegroup_variant_comparison)
{
test_liegroup_variant_equal(
VectorSpaceOperationTpl<1, double>(),
VectorSpaceOperationTpl<Eigen::Dynamic, double>(1));
test_liegroup_variant_not_equal(
VectorSpaceOperationTpl<1, double>(),
VectorSpaceOperationTpl<Eigen::Dynamic, double>(2));
}
BOOST_AUTO_TEST_SUITE_END ()
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