move the inertias from node classes to robot class

I want to be able to change a body inertia at runtime (during an init stage).

Before this commit, inertias were stored within the Node classes

class Node0 {
public:
  Node0();
  static const int id = 0;
  static const std::string joint_name;
  static const std::string body_name;
  static const Spatial::Transform Xt;
  static const int q_idx = 0;
  typedef RevoluteAxisAnyJoint Joint;
  static const int parent_id = -1;
  static const int child0_id = 1;
  static const int child1_id = -2;
  static const int child2_id = -2;
  static const int child3_id = -2;
  static const int child4_id = -2;
  static const Spatial::Inertia I; // in body frame
  Spatial::TransformT<Spatial::rm_mul_op<Spatial::RotationMatrix, Spatial::RotationMatrix>::rm> sXp;
  Eigen::Matrix<FloatType, 6, Joint::NBDOF> joint_F; // used by crba
  Joint joint;
  Body body;
};

and were static const. I could have just removed the const to get the ability to update it at runtime. The problem is that doing so does not enable me to write a simple setter like this:

template <Robot>
setInertia(Robot &robot, int node_id, const Spatial::Inertia &inertia)
{
  typedef typename Nodes<Robot, node_id>::type Node;
  node::I = inertia;
}

because the node_id, as a template parameter, should be known at compile time. A solution would be to do

setInertias(Robot &robot, std::vector<Spatial::Inertia> &inertia)
{
  // use boost::fusion::for_each to set all the inertias
}

or to use boost::fusion::for_each to iterate (at compile time) over all the nodes and on set (doing the test at run time) the one with the matching node_id.

Instead I chose to store all the inertias in an simple array instead of a fusion vector.

Benchmarks show it has no impact on performance.

We could use the same method to ease the retrieval of other values at run time: the body poses, velocities, etc. as long as all the elements have the same type, and are never used as template parameters.

Feel free to discuss, I'm not sure where all these changes will drive us.

laas / metapod

move the inertias from node classes to robot class #59