[LieGroup] Fix equality operator.

src/multibody/liegroup/cartesian-product-variant.hpp

@@ -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;

src/multibody/liegroup/cartesian-product-variant.hxx

@@ -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__

src/multibody/liegroup/cartesian-product.hpp

@@ -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;
 

src/multibody/liegroup/liegroup-generic.hpp

@@ -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);
+    }
   };
   
 }

src/multibody/liegroup/liegroup-variant-visitors.hxx

@@ -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);
+      }
     };
   }
 

unittest/liegroups.cpp

@@ -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 ()