Naive implementation First approach

Let us first define a robot joint with the following macro:

# define JOINT_REVOLUTE(classname)         \
class classname                         \
{                                       \
  public:                               \
  enum { NBDOF = 1 };                   \
  static const std::string name;        \
  static const int label;               \
  static const int positionInConf;      \
  static const Spatial::Transform Xt;   \
  static Spatial::Transform sXp;        \
  static Spatial::Transform Xj;         \
  static Spatial::Motion cj;            \
  static Spatial::Motion vj;            \
  static const vector6d S;              \
  static const vector6d dotS;           \
  static Spatial::Force f;              \
  static vector1d torque;               \
  static vector6d F;                    \
                                        \
  static void jcalc(const vector1d & qi,\ 
        const vector1d & dqi);          \
};       

A similar kind of macro may be used to declare a body. To specify the joint, the parameters are declared in the following way:

INITIALIZE_JOINT_REVOLUTE(RLEG_HIP_R);   
const std::string RLEG_HIP_R::name =     
   "RLEG_HIP_R"; 
const int RLEG_HIP_R::label = 25;
const int RLEG_HIP_R::positionInConf = 6;
const Spatial::Transform RLEG_HIP_R::Xt = 
  Spatial::Transform(                     
    matrix3dMaker(1, 0, 0,
                  0, 1, 0,
                  0, 0, 1),
    vector3d(0, -0.09, 0));

Again a similar initialization procedure can be used for the bodies. When the elements are declared and specified, to build the kinematic tree, one can use a Node defined as follows :

// Class Node. Contains a Body, 
// a Joint, and up to 3 Node children.
// Non-existant children make use  
// of the NC class (No-Child).
template< typename B,  // Body
         typename J,  // Joint
         typename C1 = NC,  // Children nodes
         typename C2 = NC,
         typename C3 = NC >
class Node
{
  public:
    typedef B Body;
    typedef J Joint;
    typedef C1 Child1;
    typedef C2 Child2;
    typedef C3 Child3;
};

Then the kinematic tree is defined recursively like this:

// Model of the robot. 
// Contains data at the global robot level 
// and the tree/ of Body/Joint
class METAPOD_DLLEXPORT Robot
{
  public:
    // Global constants or 
    // variable of the robot
    enum { NBDOF = 35 };
    static Eigen::
      Matrix< FloatType, 
      NBDOF, NBDOF > H;
    typedef Eigen::
      Matrix< FloatType, NBDOF, 1 > 
       confVector;
     // Definition of the multibody 
     // tree as a type.
 typedef 
   Node< WAIST_LINK0,
     WAIST,
     Node< WAIST_LINK1,
       WAIST_P,
       Node< WAIST_LINK2,
         WAIST_R,
         Node< WAIST_LINK3,
           CHEST,
           Node< LARM_LINK1,
             // ...
           >,
           Node< RARM_LINK1,
           // ...
     > > > >,
     Node< LLEG_LINK1,
       LLEG_HIP_R,
       Node< LLEG_LINK2,
         LLEG_HIP_P,
         Node< LLEG_LINK3,
           LLEG_HIP_Y,
           Node< LLEG_LINK4,
             LLEG_KNEE,
             Node< LLEG_LINK5,
               LLEG_ANKLE_P,
               Node< LLEG_LINK6,
                LLEG_ANKLE_R >
    > > > > >,
    Node< RLEG_LINK1,
      RLEG_HIP_R,
      Node< RLEG_LINK2,
        RLEG_HIP_P,
        Node< RLEG_LINK3,
          RLEG_HIP_Y,
          Node< RLEG_LINK4,
            RLEG_KNEE,
            Node< RLEG_LINK5,
              RLEG_ANKLE_P,
              Node< RLEG_LINK6,
                RLEG_ANKLE_R >
  > > > > > > Tree;
 };
} // end of namespace simple_humanoid

The clear disadvantage with this approach is that the structure is storing the data directly in the class itself and not in an instance of this class. In the current version of metapod this is fixed by using boost::fusion.