test/visitors: add new test for Unary and Binary visitors

unittest/visitor.cpp

@@ -13,8 +13,8 @@
 
 namespace bf = boost::fusion;
 
-struct SimpleVisitor
-: public pinocchio::fusion::JointUnaryVisitorBase<SimpleVisitor>
+struct SimpleUnaryVisitor1
+: public pinocchio::fusion::JointUnaryVisitorBase<SimpleUnaryVisitor1>
 {
 
   typedef bf::vector<const pinocchio::Model &,
@@ -29,13 +29,147 @@ struct SimpleVisitor
                    pinocchio::JointIndex jindex)
   {
     PINOCCHIO_UNUSED_VARIABLE(jdata);
+    PINOCCHIO_UNUSED_VARIABLE(model);
+    PINOCCHIO_UNUSED_VARIABLE(data);
+    BOOST_CHECK(jindex == jmodel.id());
+    std::cout << "joint name: " << jmodel.shortname() << std::endl;
+  }
+};
+
+struct SimpleUnaryVisitor2
+: public pinocchio::fusion::JointUnaryVisitorBase<SimpleUnaryVisitor2>
+{
+
+  typedef bf::vector<const pinocchio::Model &,
+                     pinocchio::Data &,
+                     pinocchio::JointIndex> ArgsType;
+  
+  template<typename JointModel>
+  static void algo(const pinocchio::JointModelBase<JointModel> & jmodel,
+                   const pinocchio::Model & model,
+                   pinocchio::Data & data,
+                   pinocchio::JointIndex jindex)
+  {
     PINOCCHIO_UNUSED_VARIABLE(model);
     PINOCCHIO_UNUSED_VARIABLE(data);
     PINOCCHIO_UNUSED_VARIABLE(jindex);
+    BOOST_CHECK(jindex == jmodel.id());
+    std::cout << "joint name: " << jmodel.shortname() << std::endl;
+  }
+};
+
+struct SimpleUnaryVisitor3
+: public pinocchio::fusion::JointUnaryVisitorBase<SimpleUnaryVisitor3>
+{
+  
+  template<typename JointModel>
+  static void algo(const pinocchio::JointModelBase<JointModel> & jmodel)
+  {
+    BOOST_CHECK(!jmodel.shortname().empty());
+    std::cout << "joint name: " << jmodel.shortname() << std::endl;
+  }
+};
+
+struct SimpleUnaryVisitor4
+: public pinocchio::fusion::JointUnaryVisitorBase<SimpleUnaryVisitor4>
+{
+  
+  template<typename JointModel>
+  static void algo(const pinocchio::JointModelBase<JointModel> & jmodel,
+                   pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata)
+  {
+    PINOCCHIO_UNUSED_VARIABLE(jdata);
+    BOOST_CHECK(!jmodel.shortname().empty());
     std::cout << "joint name: " << jmodel.shortname() << std::endl;
   }
 };
 
+struct SimpleBinaryVisitor1
+: public pinocchio::fusion::JointBinaryVisitorBase<SimpleBinaryVisitor1>
+{
+
+  typedef bf::vector<const pinocchio::Model &,
+                     pinocchio::Data &,
+                     pinocchio::JointIndex> ArgsType;
+  
+  template<typename JointModel1, typename JointModel2>
+  static void algo(const pinocchio::JointModelBase<JointModel1> & jmodel1,
+                   const pinocchio::JointModelBase<JointModel2> & jmodel2,
+                   pinocchio::JointDataBase<typename JointModel1::JointDataDerived> & jdata1,
+                   pinocchio::JointDataBase<typename JointModel2::JointDataDerived> & jdata2,
+                   const pinocchio::Model & model,
+                   pinocchio::Data & data,
+                   pinocchio::JointIndex jindex)
+  {
+    PINOCCHIO_UNUSED_VARIABLE(jdata1);
+    PINOCCHIO_UNUSED_VARIABLE(jdata2);
+    PINOCCHIO_UNUSED_VARIABLE(model);
+    PINOCCHIO_UNUSED_VARIABLE(data);
+    PINOCCHIO_UNUSED_VARIABLE(jindex);
+    BOOST_CHECK(jindex == jmodel1.id());
+    BOOST_CHECK(jindex == jmodel2.id());
+    std::cout << "joint1 name: " << jmodel1.shortname() << std::endl;
+    std::cout << "joint2 name: " << jmodel2.shortname() << std::endl;
+  }
+};
+
+struct SimpleBinaryVisitor2
+: public pinocchio::fusion::JointBinaryVisitorBase<SimpleBinaryVisitor2>
+{
+
+  typedef bf::vector<const pinocchio::Model &,
+                     pinocchio::Data &,
+                     pinocchio::JointIndex> ArgsType;
+  
+  template<typename JointModel1, typename JointModel2>
+  static void algo(const pinocchio::JointModelBase<JointModel1> & jmodel1,
+                   const pinocchio::JointModelBase<JointModel2> & jmodel2,
+                   const pinocchio::Model & model,
+                   pinocchio::Data & data,
+                   pinocchio::JointIndex jindex)
+  {
+    PINOCCHIO_UNUSED_VARIABLE(model);
+    PINOCCHIO_UNUSED_VARIABLE(data);
+    PINOCCHIO_UNUSED_VARIABLE(jindex);
+    BOOST_CHECK(jindex == jmodel1.id());
+    BOOST_CHECK(jindex == jmodel2.id());
+    std::cout << "joint1 name: " << jmodel1.shortname() << std::endl;
+    std::cout << "joint2 name: " << jmodel2.shortname() << std::endl;
+  }
+};
+
+struct SimpleBinaryVisitor3
+: public pinocchio::fusion::JointBinaryVisitorBase<SimpleBinaryVisitor3>
+{
+  template<typename JointModel1, typename JointModel2>
+  static void algo(const pinocchio::JointModelBase<JointModel1> & jmodel1,
+                   const pinocchio::JointModelBase<JointModel2> & jmodel2)
+  {
+    BOOST_CHECK(!jmodel1.shortname().empty());
+    BOOST_CHECK(!jmodel2.shortname().empty());
+    std::cout << "joint1 name: " << jmodel1.shortname() << std::endl;
+    std::cout << "joint2 name: " << jmodel2.shortname() << std::endl;
+  }
+};
+
+struct SimpleBinaryVisitor4
+: public pinocchio::fusion::JointBinaryVisitorBase<SimpleBinaryVisitor4>
+{
+  template<typename JointModel1, typename JointModel2>
+  static void algo(const pinocchio::JointModelBase<JointModel1> & jmodel1,
+                   const pinocchio::JointModelBase<JointModel2> & jmodel2,
+                   pinocchio::JointDataBase<typename JointModel1::JointDataDerived> & jdata1,
+                   pinocchio::JointDataBase<typename JointModel2::JointDataDerived> & jdata2)
+  {
+    PINOCCHIO_UNUSED_VARIABLE(jdata1);
+    PINOCCHIO_UNUSED_VARIABLE(jdata2);
+    BOOST_CHECK(!jmodel1.shortname().empty());
+    BOOST_CHECK(!jmodel2.shortname().empty());
+    std::cout << "joint1 name: " << jmodel1.shortname() << std::endl;
+    std::cout << "joint2 name: " << jmodel2.shortname() << std::endl;
+  }
+};
+
 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
 
 template<typename JointModel_> struct init;
@@ -133,7 +267,6 @@ struct init<pinocchio::JointModelMimic<JointModel_> >
   static JointModel run(const pinocchio::Model & model)
   {
     const pinocchio::JointIndex joint_id = model.getJointId(JointModel_::classname());
-    std::cout << "joint_id: " << joint_id << std::endl;
     
     JointModel jmodel(boost::get<JointModel_>(model.joints[joint_id]),1.,0.);
     
@@ -156,7 +289,40 @@ struct AppendJointToModel
   pinocchio::Model & model;
 };
 
-BOOST_AUTO_TEST_CASE(test_run_over_all_joints)
+BOOST_AUTO_TEST_CASE(test_run_over_all_joints_unary_visitor)
+{
+  using namespace pinocchio;
+
+  typedef JointCollectionDefault::JointModelVariant Variant;
+  
+  Model model;
+  boost::mpl::for_each<Variant::types>(AppendJointToModel(model));
+  Data data(model);
+  
+  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
+  {
+    SimpleUnaryVisitor1::run(model.joints[i],data.joints[i],
+                             SimpleUnaryVisitor1::ArgsType(model,data,i));
+  }
+  
+  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
+  {
+    SimpleUnaryVisitor2::run(model.joints[i],
+                             SimpleUnaryVisitor2::ArgsType(model,data,i));
+  }
+  
+  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
+  {
+    SimpleUnaryVisitor3::run(model.joints[i]);
+  }
+  
+  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
+  {
+    SimpleUnaryVisitor4::run(model.joints[i],data.joints[i]);
+  }
+}
+
+BOOST_AUTO_TEST_CASE(test_run_over_all_joints_binary_visitor)
 {
   using namespace pinocchio;
 
@@ -168,8 +334,26 @@ BOOST_AUTO_TEST_CASE(test_run_over_all_joints)
   
   for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
   {
-    SimpleVisitor::run(model.joints[i],data.joints[i],
-                       SimpleVisitor::ArgsType(model,data,i));
+    SimpleBinaryVisitor1::run(model.joints[i],model.joints[i],
+                              data.joints[i],data.joints[i],
+                              SimpleBinaryVisitor1::ArgsType(model,data,i));
+  }
+  
+  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
+  {
+    SimpleBinaryVisitor2::run(model.joints[i],model.joints[i],
+                              SimpleBinaryVisitor2::ArgsType(model,data,i));
+  }
+  
+  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
+  {
+    SimpleBinaryVisitor3::run(model.joints[i],model.joints[i]);
+  }
+  
+  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
+  {
+    SimpleBinaryVisitor4::run(model.joints[i],model.joints[i],
+                              data.joints[i],data.joints[i]);
   }
 }