joints: add operator== to JointConstraint

src/multibody/constraint-base.hpp

@@ -98,6 +98,11 @@ namespace pinocchio
     {
       return derived().motionAction(v);
     }
+    
+    bool operator==(const ConstraintBase<Derived> & other) const
+    {
+      return derived().isEqual(other.derived());
+    }
 
   }; // class ConstraintBase
   

src/multibody/joint/joint-free-flyer.hpp

@@ -94,6 +94,8 @@ namespace pinocchio
     motionAction(const MotionBase<MotionDerived> & v) const
     { return v.toActionMatrix(); }
     
+    bool isEqual(const ConstraintIdentityTpl &) const { return true; }
+    
   }; // struct ConstraintIdentityTpl
   
   template<typename Scalar, int Options, typename Vector6Like>

src/multibody/joint/joint-mimic.hpp

@@ -155,6 +155,12 @@ namespace pinocchio
     inline const Scalar & scaling() const { return m_scaling_factor; }
     inline const Constraint & constraint() const { return m_constraint; }
     
+    bool isEqual(const ScaledConstraint & other) const
+    {
+      return m_constraint == other.m_constraint
+      && m_scaling_factor == other.m_scaling_factor;
+    }
+    
   protected:
     
     Constraint m_constraint;

src/multibody/joint/joint-planar.hpp

@@ -327,6 +327,9 @@ namespace pinocchio
       
       return res;
     }
+    
+    bool isEqual(const ConstraintPlanarTpl &)  const { return true; }
+    
   }; // struct ConstraintPlanarTpl
 
   template<typename MotionDerived, typename S2, int O2>

src/multibody/joint/joint-prismatic-unaligned.hpp

@@ -328,6 +328,11 @@ namespace pinocchio
     const Vector3 & axis() const { return m_axis; }
     Vector3 & axis() { return m_axis; }
     
+    bool isEqual(const ConstraintPrismaticUnalignedTpl & other) const
+    {
+      return m_axis == other.m_axis;
+    }
+    
   protected:
     
     Vector3 m_axis;

src/multibody/joint/joint-prismatic.hpp

@@ -378,6 +378,8 @@ namespace pinocchio
       v = m.cross(Axis());
       return res;
     }
+    
+    bool isEqual(const ConstraintPrismaticTpl &) const { return true; }
 
   }; // struct ConstraintPrismaticTpl
   

src/multibody/joint/joint-revolute-unaligned.hpp

@@ -346,6 +346,11 @@ namespace pinocchio
     const Vector3 & axis() const { return m_axis; }
     Vector3 & axis() { return m_axis; }
     
+    bool isEqual(const ConstraintRevoluteUnalignedTpl & other) const
+    {
+      return m_axis == other.m_axis;
+    }
+    
   protected:
     
     Vector3 m_axis;

src/multibody/joint/joint-revolute.hpp

@@ -450,6 +450,9 @@ namespace pinocchio
       v = m.cross(Axis());
       return res;
     }
+    
+    bool isEqual(const ConstraintRevoluteTpl &) const { return true; }
+    
   }; // struct ConstraintRevoluteTpl
 
   template<typename _Scalar, int _Options, int _axis>

src/multibody/joint/joint-spherical-ZYX.hpp

@@ -166,6 +166,10 @@ namespace pinocchio
     
     // data
     Matrix3 S_minimal;
+    bool isEqual(const ConstraintSphericalZYXTpl & other) const
+    {
+      return m_S == other.m_S;
+    }
     
   }; // struct ConstraintSphericalZYXTpl
 

src/multibody/joint/joint-spherical.hpp

@@ -281,6 +281,8 @@ namespace pinocchio
       
       return res;
     }
+    
+    bool isEqual(const ConstraintSphericalTpl &) const { return true; }
 
   }; // struct ConstraintSphericalTpl
 

src/multibody/joint/joint-translation.hpp

@@ -360,6 +360,8 @@ namespace pinocchio
       return res;
     }
     
+    bool isEqual(const ConstraintTranslationTpl &) const { return true; }
+    
   }; // struct ConstraintTranslationTpl
   
   template<typename MotionDerived, typename S2, int O2>