joints: add isEqual

src/multibody/joint/joint-generic.hpp

@@ -85,6 +85,9 @@ namespace pinocchio
     typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
     typedef typename JointCollection::JointDataVariant JointDataVariant;
     
+    using Base::operator==;
+    using Base::operator!=;
+    
     JointDataVariant & toVariant() { return *static_cast<JointDataVariant*>(this); }
     const JointDataVariant & toVariant() const { return *static_cast<const JointDataVariant*>(this); }
 
@@ -122,6 +125,12 @@ namespace pinocchio
 
     static std::string classname() { return "JointData"; }
     std::string shortname() const { return ::pinocchio::shortname(*this); }
+    
+    bool isEqual(const JointDataTpl & other) const
+    {
+      return Base::isEqual(other)
+      && toVariant() == other.toVariant();
+    }
 
   };
   

src/multibody/joint/joint-mimic.hpp

@@ -297,6 +297,15 @@ namespace pinocchio
       return *this;
     }
     
+    bool isEqual(const JointDataMimic & other) const
+    {
+      return Base::isEqual(other)
+      && m_jdata_ref == other.m_jdata_ref
+      && m_scaling == other.m_scaling
+      && m_q_transform == other.m_q_transform
+      && m_v_transform == other.m_v_transform;
+    }
+    
     static std::string classname()
     {
       return std::string("JointDataMimic<") + JointData::classname() + std::string(">");