laas/metapod — pull request #59 by sbarthelemy (2013-04-04 12:06:13+00:00)
Title: 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::rm> sXp;
Eigen::Matrix 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
setInertia(Robot &robot, int node_id, const Spatial::Inertia &inertia)
{
typedef typename Nodes::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 &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.