a prototype for metapod using boost::fusion

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(mp-fusion)

find_package(qibuild)

qi_create_bin(mpfusion "fus.cpp")
qi_use_lib(mpfusion eigen3 metapod boost)

fus.cpp

#include <Eigen/Eigen>
#include <metapod/tools/common.hh>
#include <iostream>
#include <boost/fusion/sequence.hpp>
#include <boost/fusion/include/sequence.hpp>
#include <boost/fusion/include/vector.hpp>

  class RevoluteXJoint
  {
    public:
      enum {NB_DOF = 1};
      typedef Eigen::Matrix<double,6,NB_DOF> S_t;
      void set_qi(const Eigen::Matrix<double,1,1> & qi) {qi_ = qi;};
      void scalc() {};
      static S_t S();
    private:
      Eigen::Matrix<double,1,1> qi_;
      static const S_t S_;
  };
  const RevoluteXJoint::S_t  RevoluteXJoint::S_ = metapod::vector6dMaker(1, 0, 0, 0, 0, 0);
  //const Eigen::Matrix<float,6, RevoluteXJoint::NB_DOF> S_ = metapod::vector6dMaker(1, 0, 0, 0, 0, 0);
  Eigen::Matrix<double,6,RevoluteXJoint::NB_DOF> RevoluteXJoint::S() {return S_;};

  class FloatingJoint
  {
    public:
      enum {NB_DOF = 6};
      typedef Eigen::Matrix<double,6,NB_DOF> S_t;
      FloatingJoint();
      void set_qi(const Eigen::Matrix<double,NB_DOF,1> & qi) {qi_ = qi;};
      void scalc() {
          // dumb example
          S_(0, 0) = qi_[0];
          S_(1, 1) = qi_[1];
          S_(2, 2) = qi_[2];
      };
      Eigen::Matrix<double,6,NB_DOF> S() const;
    private:
      Eigen::Matrix<double,NB_DOF,1> qi_;
      Eigen::Matrix<double,6,NB_DOF> S_;
  };
  FloatingJoint::FloatingJoint() {
      S_ = S_t::Zero();
  }
  FloatingJoint::S_t FloatingJoint::S() const {return S_;};

  //the tree: not an object but a type, with only static data
  struct Node1;
  struct Node2;

  struct NoParent {};
  struct NoChild {};
  struct Node0 { // or Node<0> ?
      static const int id = 0;
      static const int q_idx = 0; // sum of NB_DOF of all the Node with id < the current one
      typedef FloatingJoint Joint;
      typedef NoParent parent;
      typedef Node1 child0;
      typedef NoChild child1;
  };

  struct Node1 {
      static const int id = 1;
      static const int q_idx = 6;
      typedef RevoluteXJoint Joint;
      typedef Node0 parent;
      typedef Node2 child0;
      typedef NoChild child1;
  };

  struct Node2 {
      static const int id = 2;
      static const int q_idx = 7;
      typedef RevoluteXJoint Joint;
      typedef Node1 parent;
      typedef NoChild child0;
      typedef NoChild child1;
  };
  typedef Node0 Tree;

  // store instance of joints (objects)
  typedef boost::fusion::vector<
      Node0::Joint,
      Node1::Joint,
      Node2::Joint> jointVector;

  // print_nb_dof: operate on the tree alone
  template<typename Node>
  struct print_nb_dof {
      static void run() {
          std::cout << "id: " << Node::id;
          std::cout << " nb_dof: " << Node::Joint::NB_DOF;
          std::cout << std::endl;
          print_nb_dof<typename Node::child0>::run();
          print_nb_dof<typename Node::child1>::run();
      }
};

template<>
struct print_nb_dof<NoChild> {
    static void run() {
    }
};

// set_qi, call scalc, and print S.
template<typename Node>
struct scalc {
    static void run(jointVector & joints, const Eigen::Matrix<double, 8, 1> & q) {
        typename Node::Joint & j = boost::fusion::at_c<Node::id>(joints);
        //const typename Node::Joint j = boost::fusion::advance_c<Node::id>(boost::fusion::begin(joints));
        j.set_qi(
                q.segment<Node::Joint::NB_DOF>(Node::q_idx));
        j.scalc();
        std::cout << j.S() << std::endl;
        scalc<typename Node::child0>::run(joints, q);
        scalc<typename Node::child1>::run(joints, q);
    }
};

template<>
struct scalc<NoChild> {
    static void run(jointVector & joints, const Eigen::Matrix<double, 8, 1> & q) {
    }
};

// print S
template<typename Node>
struct print_s {
    static void run(const jointVector & joints) {
        const typename Node::Joint & joint = boost::fusion::at_c<Node::id>(joints);
        std::cout << joint.S() << std::endl;
        print_s<typename Node::child0>::run(joints);
        print_s<typename Node::child1>::run(joints);
    }
};

template<>
struct print_s<NoChild> {
    static void run(const jointVector & joints) {
    }
};
int main()
{
    // test joints alone
    Eigen::Matrix<double, 8, 1> q;
    q << 1, 2, 3, 4, 5, 6, 7, 8;
    FloatingJoint fj;
    fj.set_qi(q.segment<FloatingJoint::NB_DOF>(0));
    fj.scalc();
    std::cout << fj.S() << std::endl;
    RevoluteXJoint rj;
    rj.set_qi(q.segment<RevoluteXJoint::NB_DOF>(6));
    rj.scalc();
    std::cout << rj.S() << std::endl;
    // test the tree
    print_nb_dof<Tree>::run();
    // test the joints storage
    jointVector joints;
    scalc<Tree>::run(joints, q);
    // check the result has not been forgotten
    print_s<Tree>::run(joints);
};

qiproject.xml

<project name="mp-fusion">
  <!-- Add your project dependencies here -->
  <depends buildtime="true" runtime="true" names="metapod" />
</project>