visitor.cpp

Go to the documentation of this file.

//
// Copyright (c) 2015-2019 CNRS INRIA
//
 
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/visitor.hpp"
 
#include <iostream>
 
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
 
namespace bf = boost::fusion;
 
struct SimpleUnaryVisitor1 : public pinocchio::fusion::JointUnaryVisitorBase<SimpleUnaryVisitor1>
{
 
  typedef bf::vector<const pinocchio::Model &, pinocchio::Data &, pinocchio::JointIndex> ArgsType;
 
  template<typename JointModel>
  static void algo(
    const pinocchio::JointModelBase<JointModel> & jmodel,
    pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
    const pinocchio::Model & model,
    pinocchio::Data & data,
    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;
    std::cout << "jdata1 name: " << jdata1.classname() << std::endl;
    std::cout << "jdata2 name: " << jdata2.classname() << std::endl;
  }
};
 
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
 
template<typename JointModel_>
struct init;
 
template<typename JointModel_>
struct init
{
  static JointModel_ run(const pinocchio::Model & /* model*/)
  {
    JointModel_ jmodel;
    return jmodel;
  }
};
 
template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options>>
{
  typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options> JointModel;
 
  static JointModel run(const pinocchio::Model & /* model*/)
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());
 
    return jmodel;
  }
};
 
template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options>>
{
  typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options> JointModel;
 
  static JointModel run(const pinocchio::Model & /* model*/)
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());
 
    return jmodel;
  }
};
 
template<typename Scalar, int Options>
struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options>>
{
  typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options> JointModel;
 
  static JointModel run(const pinocchio::Model & /* model*/)
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());
 
    return jmodel;
  }
};
 
template<typename Scalar, int Options, template<typename, int> class JointCollection>
struct init<pinocchio::JointModelTpl<Scalar, Options, JointCollection>>
{
  typedef pinocchio::JointModelTpl<Scalar, Options, JointCollection> JointModel;
 
  static JointModel run(const pinocchio::Model & /* model*/)
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
    JointModel jmodel((JointModelRX()));
 
    return jmodel;
  }
};
 
template<typename Scalar, int Options, template<typename, int> class JointCollection>
struct init<pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection>>
{
  typedef pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection> JointModel;
 
  static JointModel run(const pinocchio::Model & /* model*/)
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
    typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 1> JointModelRY;
    typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 2> JointModelRZ;
 
    JointModel jmodel(JointModelRX(), pinocchio::SE3::Random());
    jmodel.addJoint(JointModelRY(), pinocchio::SE3::Random());
    jmodel.addJoint(JointModelRZ(), pinocchio::SE3::Random());
 
    return jmodel;
  }
};
 
template<typename JointModel_>
struct init<pinocchio::JointModelMimic<JointModel_>>
{
  typedef pinocchio::JointModelMimic<JointModel_> JointModel;
 
  static JointModel run(const pinocchio::Model & model)
  {
    const pinocchio::JointIndex joint_id = model.getJointId(JointModel_::classname());
 
    JointModel jmodel(boost::get<JointModel_>(model.joints[joint_id]), 1., 0.);
 
    return jmodel;
  }
};
 
struct AppendJointToModel
{
  AppendJointToModel(pinocchio::Model & model)
  : model(model)
  {
  }
 
  template<typename JointModel>
  void operator()(const pinocchio::JointModelBase<JointModel> &) const
  {
    JointModel jmodel = init<JointModel>::run(model);
    model.addJoint(model.joints.size() - 1, jmodel, pinocchio::SE3::Random(), jmodel.classname());
  }
 
  pinocchio::Model & model;
};
 
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;
 
  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)
  {
    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]);
  }
}
 
BOOST_AUTO_TEST_SUITE_END()