Upgrade the code design to be compatible with CoSimA

[xwavex]

In the long run this code should be upgraded to the new more user-friendly and more stable architecture: CoSimA.

See https://github.com/cogimon/rtt-gazebo-robot-sim/tree/kinematic_chains as reference