Rotational velocity kinematics mode

Hi @mirkowaechter here is the implementation I wrote to compute the rotational velocity of a robot node in kinematics mode.

It seems to work for the Armar4 when I replay some motion with the MMM player.

It computes the floating base velcocity from 2 rotation matrices taken at different time. Then, it adds the velocity induced by the robot joints computed with a simox jacobian.

The only problem of the proposed implementation, is that it assumes there is a NodeSetName called "All". A workaround would be to create a new NodeSetName with all the joints. But before doing that, I let you check the current implementation.