core: add hasSameIndexes to JointModelTpl

src/multibody/joint/joint-basic-visitors.hpp

@@ -187,6 +187,19 @@ namespace pinocchio
                const JointModelBase<JointModelDerived> & jmodel);
 
   
+  /**
+   * @brief      Check whether JointModelTpl<Scalar,...> has the indexes than another JointModelDerived
+   *
+   * @param[in]  jmodel_generic  The generic joint model containing a variant.
+   * @param[in]  jmodel  The other joint modelto compare with
+   *
+   * @return     True if the two joints have the same indexes.
+   */
+  template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
+  bool hasSameIndexes(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic,
+                      const JointModelBase<JointModelDerived> & jmodel);
+
+  
   //
   // Visitors on JointDatas
   //

src/multibody/joint/joint-basic-visitors.hxx

@@ -311,7 +311,7 @@ namespace pinocchio
     static bool algo(const JointModelBase<JointModel> & jmodel_lhs,
                      const JointModelDerived & jmodel_rhs)
     {
-      return jmodel_lhs == jmodel_rhs;
+      return jmodel_lhs.derived() == jmodel_rhs;
     }
 
   };
@@ -324,6 +324,29 @@ namespace pinocchio
     return Algo::run(jmodel_generic,typename Algo::ArgsType(boost::ref(jmodel.derived())));
   }
 
+  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
+  struct JointModelHasSameIndexesVisitor
+  : fusion::JointUnaryVisitorBase< JointModelHasSameIndexesVisitor<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.derived().hasSameIndexes(jmodel_rhs);
+    }
+
+  };
+
+  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
+  bool hasSameIndexes(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic,
+                      const JointModelBase<JointModelDerived> & jmodel)
+  {
+    typedef JointModelHasSameIndexesVisitor<Scalar,Options,JointCollectionTpl,JointModelDerived> Algo;
+    return Algo::run(jmodel_generic,typename Algo::ArgsType(boost::ref(jmodel.derived())));
+  }
+
   //
   // Visitors on JointDatas
   //

unittest/joint-generic.cpp

@@ -43,6 +43,7 @@ void test_joint_methods(JointModelBase<JointModel> & jmodel,
   pinocchio::JointModel jma(jmodel);
   BOOST_CHECK(jmodel == jma);
   BOOST_CHECK(jma == jmodel);
+  BOOST_CHECK(jma.hasSameIndexes(jmodel));
   
   pinocchio::JointData jda(jdata);
   BOOST_CHECK(jda == jdata);