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.