Created
November 22, 2012 11:39
-
-
Save sbarthelemy/4130724 to your computer and use it in GitHub Desktop.
a prototype for metapod using boost::fusion
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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); | |
}; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
<project name="mp-fusion"> | |
<!-- Add your project dependencies here --> | |
<depends buildtime="true" runtime="true" names="metapod" /> | |
</project> |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment